The following document contains the results of PMD's CPD 6.55.0.
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 2099 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 2063 |
final KnownBiasTurntableGyroscopeCalibratorListener listener) {
this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets known x-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @return known x-coordinate of gyroscope bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @param biasX known x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @return known y-coordinate of gyroscope bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @param biasY known y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @return known z-coordinate of gyroscope bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @param biasZ known z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x-coordinate of gyroscope bias.
*
* @return known x-coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedX() {
return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known x-coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(biasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known x-coordinate of gyroscope bias.
*
* @param biasX known x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final AngularSpeed biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
}
/**
* Gets known y-coordinate of gyroscope bias.
*
* @return known y-coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedY() {
return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known y-coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(biasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param biasY known y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final AngularSpeed biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAngularSpeed(biasY);
}
/**
* Gets known z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return known z-coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedZ() {
return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known z-coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(biasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known z-coordinate of gyroscope bias.
*
* @param biasZ known z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Sets known bias coordinates of gyroscope expressed in radians
* per second (rad/s).
*
* @param biasX known x-coordinate of gyroscope bias.
* @param biasY known y-coordinate of gyroscope bias.
* @param biasZ known z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(
final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known bias coordinates of gyroscope.
*
* @param biasX known x-coordinate of gyroscope bias.
* @param biasY known y-coordinate of gyroscope bias.
* @param biasZ known z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
this.biasY = convertAngularSpeed(biasY);
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Gets known gyroscope bias.
*
* @return known gyroscope bias.
*/
public AngularSpeedTriad getBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known gyroscope bias.
*
* @param result instance where result will be stored.
*/
public void getBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known gyroscope bias.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setBias(final AngularSpeedTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of gyroscope bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param bias known bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known gyroscope bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @return known gyroscope bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known gyroscope bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known gyroscope bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @param bias known gyroscope bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @return constant rotation rate of turntable.
*/
public double getTurntableRotationRate() {
return turntableRotationRate;
}
/**
* Sets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(final double turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
if (turntableRotationRate <= 0.0) {
throw new IllegalArgumentException();
}
this.turntableRotationRate = turntableRotationRate;
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @return constant rotation rate of turntable.
*/
public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @param result instance where result will be stored.
*/
public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
result.setValue(turntableRotationRate);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets constant rotation rate at which the turntable is spinning.
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
}
/**
* Gets time interval between measurements being captured expressed in
* seconds (s).
*
* @return time interval between measurements.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between measurements being captured expressed in
* seconds (s).
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval <= 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between measurements being captured.
*
* @return time interval between measurements.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between measurements being captured.
*
* @param result instance where result will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between measurements being captured.
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
setTimeInterval(convertTime(timeInterval));
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 1022 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 2103 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 2067 |
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx, accelerometerMyz,
accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets x-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @return x-coordinate of gyroscope known bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets x-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @param biasX x-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets y-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @return y-coordinate of gyroscope known bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets y-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @param biasY y-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets z-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @return z-coordinate of gyroscope known bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets z-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @param biasZ z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets x-coordinate of gyroscope known bias.
*
* @return x-coordinate of gyroscope known bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedX() {
return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets x-coordinate of gyroscope known bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(biasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets x-coordinate of gyroscope known bias.
*
* @param biasX x-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final AngularSpeed biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
}
/**
* Gets y-coordinate of gyroscope known bias.
*
* @return y-coordinate of gyroscope known bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedY() {
return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets y-coordinate of gyroscope known bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(biasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets y-coordinate of gyroscope known bias.
*
* @param biasY y-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final AngularSpeed biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAngularSpeed(biasY);
}
/**
* Gets z-coordinate of gyroscope known bias.
*
* @return initial z-coordinate of gyroscope known bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedZ() {
return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets z-coordinate of gyroscope known bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(biasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets z-coordinate of gyroscope known bias.
*
* @param biasZ z-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Sets known bias coordinates of gyroscope expressed in
* radians per second (rad/s).
*
* @param biasX x-coordinate of gyroscope known bias.
* @param biasY y-coordinate of gyroscope known bias.
* @param biasZ z-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(
final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known bias coordinates of gyroscope.
*
* @param biasX x-coordinate of gyroscope known bias.
* @param biasY y-coordinate of gyroscope known bias.
* @param biasZ z-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
this.biasY = convertAngularSpeed(biasY);
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Gets known gyroscope bias.
*
* @return known gyroscope bias.
*/
public AngularSpeedTriad getBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known gyroscope bias.
*
* @param result instance where result will be stored.
*/
public void getBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known gyroscope bias.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setBias(final AngularSpeedTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets gyroscope known bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of gyroscope known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets gyroscope known bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets gyroscope known bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param bias known bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets gyroscope known bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @return initial gyroscope bias to be used to find a solution as a
* column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets gyroscope known bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets gyroscope known bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @param bias gyroscope known bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 1018 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 1021 |
final Matrix accelerometerMa, final KnownBiasEasyGyroscopeCalibratorListener listener) {
this(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
this.listener = listener;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx, accelerometerMyz,
accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets x-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @return x-coordinate of gyroscope known bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets x-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @param biasX x-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets y-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @return y-coordinate of gyroscope known bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets y-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @param biasY y-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets z-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @return z-coordinate of gyroscope known bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets z-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @param biasZ z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets x-coordinate of gyroscope known bias.
*
* @return x-coordinate of gyroscope known bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedX() {
return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets x-coordinate of gyroscope known bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(biasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets x-coordinate of gyroscope known bias.
*
* @param biasX x-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final AngularSpeed biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
}
/**
* Gets y-coordinate of gyroscope known bias.
*
* @return y-coordinate of gyroscope known bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedY() {
return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets y-coordinate of gyroscope known bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(biasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets y-coordinate of gyroscope known bias.
*
* @param biasY y-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final AngularSpeed biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAngularSpeed(biasY);
}
/**
* Gets z-coordinate of gyroscope known bias.
*
* @return initial z-coordinate of gyroscope known bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedZ() {
return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets z-coordinate of gyroscope known bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(biasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets z-coordinate of gyroscope known bias.
*
* @param biasZ z-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Sets known bias coordinates of gyroscope expressed in
* radians per second (rad/s).
*
* @param biasX x-coordinate of gyroscope known bias.
* @param biasY y-coordinate of gyroscope known bias.
* @param biasZ z-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(
final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known bias coordinates of gyroscope.
*
* @param biasX x-coordinate of gyroscope known bias.
* @param biasY y-coordinate of gyroscope known bias.
* @param biasZ z-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
this.biasY = convertAngularSpeed(biasY);
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Gets known gyroscope bias.
*
* @return known gyroscope bias.
*/
public AngularSpeedTriad getBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known gyroscope bias.
*
* @param result instance where result will be stored.
*/
public void getBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known gyroscope bias.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setBias(final AngularSpeedTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets gyroscope known bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of gyroscope known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets gyroscope known bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets gyroscope known bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param bias known bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets gyroscope known bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @return initial gyroscope bias to be used to find a solution as a
* column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets gyroscope known bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets gyroscope known bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @param bias gyroscope known bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 2103 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 1025 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 2067 |
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets known x-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @return known x-coordinate of gyroscope bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @param biasX known x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @return known y-coordinate of gyroscope bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @param biasY known y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @return known z-coordinate of gyroscope bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*
* @param biasZ known z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x-coordinate of gyroscope bias.
*
* @return known x-coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedX() {
return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known x-coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(biasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known x-coordinate of gyroscope bias.
*
* @param biasX known x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final AngularSpeed biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
}
/**
* Gets known y-coordinate of gyroscope bias.
*
* @return known y-coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedY() {
return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known y-coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(biasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param biasY known y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final AngularSpeed biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAngularSpeed(biasY);
}
/**
* Gets known z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return known z-coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedZ() {
return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known z-coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(biasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known z-coordinate of gyroscope bias.
*
* @param biasZ known z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Sets known bias coordinates of gyroscope expressed in radians
* per second (rad/s).
*
* @param biasX known x-coordinate of gyroscope bias.
* @param biasY known y-coordinate of gyroscope bias.
* @param biasZ known z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(
final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known bias coordinates of gyroscope.
*
* @param biasX known x-coordinate of gyroscope bias.
* @param biasY known y-coordinate of gyroscope bias.
* @param biasZ known z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
this.biasY = convertAngularSpeed(biasY);
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Gets known gyroscope bias.
*
* @return known gyroscope bias.
*/
public AngularSpeedTriad getBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known gyroscope bias.
*
* @param result instance where result will be stored.
*/
public void getBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known gyroscope bias.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setBias(final AngularSpeedTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of gyroscope bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param bias known bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known gyroscope bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @return known gyroscope bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known gyroscope bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known gyroscope bias as a column matrix.
* Matrix values are expressed in radians per second (rad/s).
*
* @param bias known gyroscope bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 1051 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 2115 |
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial x-coordinate of gyroscope bias.
*/
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial y-coordinate of gyroscope bias.
*/
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of gyroscope bias ot be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial z-coordinate of gyroscope bias.
*/
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial x-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedX() {
return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(initialBiasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial y-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedY() {
return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(initialBiasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAngularSpeed(initialBiasY);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial z-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedZ() {
return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(initialBiasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution
* expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
this.initialBiasY = convertAngularSpeed(initialBiasY);
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of initial gyroscope bias.
*/
public double[] getInitialBias() {
final double[] result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @return initial gyroscope bias to be used to find a solution as a
* column matrix.
*/
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* a column matrix with values expressed in radians per second (rad/s).
*
* @param initialBias initial gyroscope bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @return initial bias coordinates.
*/
public AngularSpeedTriad getInitialBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @param result instance where result will be stored.
*/
public void getInitialBiasAsTriad(AngularSpeedTriad result) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 1052 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 2154 |
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX,
final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy,
final double accelerometerSz, final double accelerometerMxy,
final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx,
final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial x-coordinate of gyroscope bias.
*/
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial y-coordinate of gyroscope bias.
*/
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of gyroscope bias ot be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial z-coordinate of gyroscope bias.
*/
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial x-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedX() {
return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(initialBiasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial y-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedY() {
return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(initialBiasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAngularSpeed(initialBiasY);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial z-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedZ() {
return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(initialBiasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution
* expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
this.initialBiasY = convertAngularSpeed(initialBiasY);
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of initial gyroscope bias.
*/
public double[] getInitialBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @return initial gyroscope bias to be used to find a solution as a
* column matrix.
*/
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* a column matrix.
* Values are expressed in radians per second (rad/s).
*
* @param initialBias initial gyroscope bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @return initial bias coordinates.
*/
public AngularSpeedTriad getInitialBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @param result instance where result will be stored.
*/
public void getInitialBiasAsTriad(AngularSpeedTriad result) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 2111 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 2150 |
final RobustTurntableGyroscopeCalibratorListener listener) {
this(convertPosition(position), turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial x-coordinate of gyroscope bias.
*/
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial y-coordinate of gyroscope bias.
*/
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of gyroscope bias ot be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial z-coordinate of gyroscope bias.
*/
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial x-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedX() {
return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(initialBiasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial y-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedY() {
return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(initialBiasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAngularSpeed(initialBiasY);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial z-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedZ() {
return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(initialBiasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution
* expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
this.initialBiasY = convertAngularSpeed(initialBiasY);
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of initial gyroscope bias.
*/
public double[] getInitialBias() {
final double[] result = new double[BodyKinematics.COMPONENTS]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 1048 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 1047 |
final Matrix accelerometerMa, final EasyGyroscopeCalibratorListener listener) {
this(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
this.listener = listener;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX,
final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy,
final double accelerometerSz, final double accelerometerMxy,
final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx,
final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial x-coordinate of gyroscope bias.
*/
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial y-coordinate of gyroscope bias.
*/
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of gyroscope bias ot be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial z-coordinate of gyroscope bias.
*/
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial x-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedX() {
return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(initialBiasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial y-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedY() {
return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(initialBiasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAngularSpeed(initialBiasY);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial z-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedZ() {
return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(initialBiasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution
* expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
this.initialBiasY = convertAngularSpeed(initialBiasY);
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of initial gyroscope bias.
*/
public double[] getInitialBias() {
final var result = new double[BodyKinematics.COMPONENTS]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 1052 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 2115 |
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX,
final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy,
final double accelerometerSz, final double accelerometerMxy,
final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx,
final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial x-coordinate of gyroscope bias.
*/
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial y-coordinate of gyroscope bias.
*/
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of gyroscope bias ot be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial z-coordinate of gyroscope bias.
*/
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial x-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedX() {
return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(initialBiasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial y-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedY() {
return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(initialBiasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAngularSpeed(initialBiasY);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial z-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedZ() {
return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(initialBiasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution
* expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
this.initialBiasY = convertAngularSpeed(initialBiasY);
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of initial gyroscope bias.
*/
public double[] getInitialBias() {
final var result = new double[BodyKinematics.COMPONENTS]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 1051 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 2154 |
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial x-coordinate of gyroscope bias.
*/
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial y-coordinate of gyroscope bias.
*/
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of gyroscope bias ot be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial z-coordinate of gyroscope bias.
*/
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find
* a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial x-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedX() {
return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(initialBiasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial y-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedY() {
return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(initialBiasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAngularSpeed(initialBiasY);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @return initial z-coordinate of gyroscope bias.
*/
public AngularSpeed getInitialBiasAngularSpeedZ() {
return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param result instance where result data will be stored.
*/
public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(initialBiasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find a
* solution.
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution
* expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(
final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
this.initialBiasY = convertAngularSpeed(initialBiasY);
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of initial gyroscope bias.
*/
public double[] getInitialBias() {
final double[] result = new double[BodyKinematics.COMPONENTS]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2239 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 497 |
this(measurements, commonAxisUsed, initialBias, initialMg, initialGg);
this.listener = listener;
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial x-coordinate of gyroscope bias.
*/
@Override
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial y-coordinate of gyroscope bias.
*/
@Override
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*
* @return initial z-coordinate of gyroscope bias.
*/
@Override
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a solution.
*
* @return initial x-coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getInitialBiasAngularSpeedX() {
return new AngularSpeed(initialBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial x-coordinate of gyroscope bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(initialBiasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial x-coordinate of gyroscope bias to be used to find a solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final AngularSpeed initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a solution.
*
* @return initial y-coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getInitialBiasAngularSpeedY() {
return new AngularSpeed(initialBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial y-coordinate of gyroscope bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(initialBiasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial y-coordinate of gyroscope bias to be used to find a solution.
*
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final AngularSpeed initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAngularSpeed(initialBiasY);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a solution.
*
* @return initial z-coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getInitialBiasAngularSpeedZ() {
return new AngularSpeed(initialBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial z-coordinate of gyroscope bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(initialBiasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial z-coordinate of gyroscope bias to be used to find a solution.
*
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final AngularSpeed initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution
* expressed in radians per second (rad/s).
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final double initialBiasX, final double initialBiasY, final double initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution.
*
* @param initialBiasX initial x-coordinate of gyroscope bias.
* @param initialBiasY initial y-coordinate of gyroscope bias.
* @param initialBiasZ initial z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(
final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
this.initialBiasY = convertAngularSpeed(initialBiasY);
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial bias to be used to find a solution as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* Values are expressed in radians per second (rad/s).
*
* @return initial bias to be used to find a solution as a column matrix.
*/
@Override
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* Values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial bias to be used to find a solution as a column matrix
* with values expressed in radians per second (rad/s).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @return initial bias coordinates.
*/
@Override
public AngularSpeedTriad getInitialBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution.
*
* @param initialBias initial bias coordinates to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final AngularSpeedTriad initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities) and containing the standard
* deviations of accelerometer and gyroscope measurements.
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<StandardDeviationFrameBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1117 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1073 |
this(convertAcceleration(groundTruthGravityNorm), measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Gets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() {
return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasXAsAcceleration(final Acceleration result) {
result.setValue(biasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets x-coordinate of known accelerometer bias.
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final Acceleration biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
}
/**
* Gets y-coordinate of known accelerometer bias.
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasYAsAcceleration() {
return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets y-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasYAsAcceleration(final Acceleration result) {
result.setValue(biasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets y-coordinate of known accelerometer bias.
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final Acceleration biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAcceleration(biasY);
}
/**
* Gets z-coordinate of known accelerometer bias.
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasZAsAcceleration() {
return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets z-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasZAsAcceleration(final Acceleration result) {
result.setValue(biasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets z-coordinate of known accelerometer bias to be used to find a solution.
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAcceleration(biasZ);
}
/**
* Sets known bias coordinates of accelerometer expressed in meters
* per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @param biasY y-coordinate of known accelerometer bias.
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known bias coordinates of accelerometer.
*
* @param biasX x-coordinate of known accelerometer bias.
* @param biasY y-coordinate of known accelerometer bias.
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
this.biasY = convertAcceleration(biasY);
this.biasZ = convertAcceleration(biasZ);
}
/**
* Gets known accelerometer bias.
*
* @return known accelerometer bias.
*/
@Override
public AccelerationTriad getBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known accelerometer bias.
*
* @param result instance where result will be stored.
*/
@Override
public void getBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known accelerometer bias.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBias(final AccelerationTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known bias to be set
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known bias as a column matrix.
*
* @return known bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known bias as an array.
*
* @param bias bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets ground truth gravity norm to be expected at location where measurements have been made,
* expressed in meter per squared second (m/s^2).
*
* @return ground truth gravity norm or null.
*/
public Double getGroundTruthGravityNorm() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2730 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 486 |
this(measurements, commonAxisUsed, magneticModel, initialHardIron, initialMm);
this.listener = listener;
}
/**
* Gets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronX() {
return initialHardIronX;
}
/**
* Sets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
}
/**
* Gets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronY() {
return initialHardIronY;
}
/**
* Sets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = initialHardIronY;
}
/**
* Gets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronZ() {
return initialHardIronZ;
}
/**
* Sets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in meters Teslas (T).
*
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = initialHardIronZ;
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Sets initial hard-iron bias coordinates of magnetometer used to find
* a solution expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
this.initialHardIronY = initialHardIronY;
this.initialHardIronZ = initialHardIronZ;
}
/**
* Sets initial hard iron coordinates of magnetometer used to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @return initial hard-iron.
*/
@Override
public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
initialHardIronX, initialHardIronY, initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial hard-iron used to find a solution.
*
* @param initialHardIron initial hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getInitialHardIron(result);
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getInitialHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialHardIronX;
result[1] = initialHardIronY;
result[2] = initialHardIronZ;
}
/**
* Sets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron[0];
initialHardIronY = initialHardIron[1];
initialHardIronZ = initialHardIron[2];
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @return initial hard-iron bias to be used to find a solution as a
* column matrix.
*/
@Override
public Matrix getInitialHardIronAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getInitialHardIronAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialHardIronAsMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialHardIronX);
result.setElementAtIndex(1, initialHardIronY);
result.setElementAtIndex(2, initialHardIronZ);
}
/**
* Sets initial hard-iron bias to be used to find a solution as a column
* matrix with values expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron.getElementAtIndex(0);
initialHardIronY = initialHardIron.getElementAtIndex(1);
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2722 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 480 |
this(measurements, commonAxisUsed, magneticModel, hardIron, initialMm);
this.listener = listener;
}
/**
* Gets x-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return x-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronX() {
return hardIronX;
}
/**
* Sets x-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronX x coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final double hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
}
/**
* Gets y-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return y-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronY() {
return hardIronY;
}
/**
* Sets y-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronY y coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final double hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = hardIronY;
}
/**
* Gets z-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return z-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronZ() {
return hardIronZ;
}
/**
* Sets z-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronZ z coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = hardIronZ;
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @return x coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known x-coordinate of magnetometer hard-iron.
*
* @param hardIronX known x-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @return y coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known y-coordinate of magnetometer hard-iron.
*
* @param hardIronY known y-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = convertMagneticFluxDensity(hardIronY);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @return z coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known z-coordinate of magnetometer hard-iron.
*
* @param hardIronZ known z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Sets known hard-iron bias coordinates of magnetometer expressed
* in Teslas (T).
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
this.hardIronY = hardIronY;
this.hardIronZ = hardIronZ;
}
/**
* Sets known hard-iron coordinates.
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
this.hardIronY = convertMagneticFluxDensity(hardIronY);
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Gets known hard-iron.
*
* @return known hard-iron.
*/
@Override
public MagneticFluxDensityTriad getHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
}
/**
* Gets known hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known hard-iron.
*
* @param hardIron hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getHardIron(result);
return result;
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = hardIronX;
result[1] = hardIronY;
result[2] = hardIronZ;
}
/**
* Sets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param hardIron known hard-iron bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setHardIron(final double[] hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
hardIronX = hardIron[0];
hardIronY = hardIron[1];
hardIronZ = hardIron[2];
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @return hard-iron bias as a column matrix.
*/
@Override
public Matrix getHardIronMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getHardIronMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getHardIronMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, hardIronX);
result.setElementAtIndex(1, hardIronY);
result.setElementAtIndex(2, hardIronZ);
}
/**
* Sets known hard-iron bias.
*
* @param hardIron magnetometer hard-iron bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setHardIron(final Matrix hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
hardIronX = hardIron.getElementAtIndex(0);
hardIronY = hardIron.getElementAtIndex(1);
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 3758 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 1824 |
}
/**
* Gets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() {
return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasXAsAcceleration(final Acceleration result) {
result.setValue(biasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets x-coordinate of known accelerometer bias.
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final Acceleration biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
}
/**
* Gets y-coordinate of known accelerometer bias.
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasYAsAcceleration() {
return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets y-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasYAsAcceleration(final Acceleration result) {
result.setValue(biasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets y-coordinate of known accelerometer bias.
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final Acceleration biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAcceleration(biasY);
}
/**
* Gets z-coordinate of known accelerometer bias.
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasZAsAcceleration() {
return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets z-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasZAsAcceleration(final Acceleration result) {
result.setValue(biasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets z-coordinate of known accelerometer bias to be used to find a solution.
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAcceleration(biasZ);
}
/**
* Sets known bias coordinates of accelerometer expressed in meters
* per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @param biasY y-coordinate of known accelerometer bias.
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known bias coordinates of accelerometer.
*
* @param biasX x-coordinate of known accelerometer bias.
* @param biasY y-coordinate of known accelerometer bias.
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(
final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
this.biasY = convertAcceleration(biasY);
this.biasZ = convertAcceleration(biasZ);
}
/**
* Gets known accelerometer bias.
*
* @return known accelerometer bias.
*/
@Override
public AccelerationTriad getBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known accelerometer bias.
*
* @param result instance where result will be stored.
*/
@Override
public void getBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known accelerometer bias.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBias(final AccelerationTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known bias to be set
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known bias as a column matrix.
*
* @return known bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known bias as an array.
*
* @param bias bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4086 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2041 |
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasXAsAcceleration() {
return new Acceleration(initialBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasXAsAcceleration(final Acceleration result) {
result.setValue(initialBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final Acceleration initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAcceleration(initialBiasX);
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasYAsAcceleration() {
return new Acceleration(initialBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasYAsAcceleration(final Acceleration result) {
result.setValue(initialBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final Acceleration initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAcceleration(initialBiasY);
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasZAsAcceleration() {
return new Acceleration(initialBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasZAsAcceleration(final Acceleration result) {
result.setValue(initialBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final Acceleration initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAcceleration(initialBiasZ);
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution
* expressed in meters per squared second (m/s^2).
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final double initialBiasX, final double initialBiasY, final double initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final Acceleration initialBiasX, final Acceleration initialBiasY,
final Acceleration initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAcceleration(initialBiasX);
this.initialBiasY = convertAcceleration(initialBiasY);
this.initialBiasZ = convertAcceleration(initialBiasZ);
}
/**
* Gets initial bias coordinates of accelerometer used to find a solution.
*
* @return initial bias coordinates.
*/
@Override
public AccelerationTriad getInitialBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of accelerometer used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution.
*
* @param initialBias initial bias coordinates to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final AccelerationTriad initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
initialBiasX = convertAcceleration(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAcceleration(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* Values are expressed in meters per squared second (m/s^2).
*
* @return initial bias to be used to find a solution as a column matrix.
*/
@Override
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* Values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial bias to be used to find a solution as a column matrix with
* values expressed in meters per squared second (m/s^2).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1137 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2724 |
}
/**
* Gets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronX() {
return hardIronX;
}
/**
* Sets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronX known x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final double hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
}
/**
* Gets known y-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronY() {
return hardIronY;
}
/**
* Sets known y-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronY known y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final double hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = hardIronY;
}
/**
* Gets known z-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronZ() {
return hardIronZ;
}
/**
* Sets known z-coordinate of magnetometer hard-iron bias.
* This is expressed in meters Teslas (T).
*
* @param hardIronZ known z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = hardIronZ;
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @return x coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known x-coordinate of magnetometer hard-iron.
*
* @param hardIronX known x-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @return y coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known y-coordinate of magnetometer hard-iron.
*
* @param hardIronY known y-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = convertMagneticFluxDensity(hardIronY);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @return z coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known z-coordinate of magnetometer hard-iron.
*
* @param hardIronZ known z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Sets known hard-iron bias coordinates of magnetometer expressed in
* Teslas (T).
*
* @param hardIronX x-coordinate of magnetometer
* known hard-iron bias.
* @param hardIronY y-coordinate of magnetometer
* known hard-iron bias.
* @param hardIronZ z-coordinate of magnetometer
* known hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
this.hardIronY = hardIronY;
this.hardIronZ = hardIronZ;
}
/**
* Sets known hard-iron coordinates.
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
this.hardIronY = convertMagneticFluxDensity(hardIronY);
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Gets known hard-iron.
*
* @return known hard-iron.
*/
@Override
public MagneticFluxDensityTriad getHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
}
/**
* Gets known hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known hard-iron.
*
* @param hardIron hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getHardIron(result);
return result;
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = hardIronX;
result[1] = hardIronY;
result[2] = hardIronZ;
}
/**
* Sets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param hardIron known hard-iron.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setHardIron(final double[] hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
hardIronX = hardIron[0];
hardIronY = hardIron[1];
hardIronZ = hardIron[2];
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @return known hard-iron bias as a column matrix.
*/
@Override
public Matrix getHardIronMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getHardIronMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getHardIronMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, hardIronX);
result.setElementAtIndex(1, hardIronY);
result.setElementAtIndex(2, hardIronZ);
}
/**
* Sets known hard-iron bias as a column matrix.
*
* @param hardIron known hard-iron bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setHardIron(final Matrix hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
hardIronX = hardIron.getElementAtIndex(0);
hardIronY = hardIron.getElementAtIndex(1);
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1194 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2732 |
}
/**
* Gets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronX() {
return initialHardIronX;
}
/**
* Sets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
}
/**
* Gets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronY() {
return initialHardIronY;
}
/**
* Sets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = initialHardIronY;
}
/**
* Gets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronZ() {
return initialHardIronZ;
}
/**
* Sets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in meters Teslas (T).
*
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = initialHardIronZ;
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Sets initial hard-iron bias coordinates of magnetometer used to find
* a solution expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
this.initialHardIronY = initialHardIronY;
this.initialHardIronZ = initialHardIronZ;
}
/**
* Sets initial hard iron coordinates of magnetometer used to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @return initial hard-iron.
*/
@Override
public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
initialHardIronX, initialHardIronY, initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial hard-iron used to find a solution.
*
* @param initialHardIron initial hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getInitialHardIron(result);
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getInitialHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialHardIronX;
result[1] = initialHardIronY;
result[2] = initialHardIronZ;
}
/**
* Sets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron[0];
initialHardIronY = initialHardIron[1];
initialHardIronZ = initialHardIron[2];
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @return initial hard-iron bias to be used to find a solution as a
* column matrix.
*/
@Override
public Matrix getInitialHardIronAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getInitialHardIronAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialHardIronAsMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialHardIronX);
result.setElementAtIndex(1, initialHardIronY);
result.setElementAtIndex(2, initialHardIronZ);
}
/**
* Sets initial hard-iron bias to be used to find a solution as a column
* matrix with values expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron.getElementAtIndex(0);
initialHardIronY = initialHardIron.getElementAtIndex(1);
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 488 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1302 |
}
/**
* Gets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronX() {
return initialHardIronX;
}
/**
* Sets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
}
/**
* Gets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronY() {
return initialHardIronY;
}
/**
* Sets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = initialHardIronY;
}
/**
* Gets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronZ() {
return initialHardIronZ;
}
/**
* Sets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in meters Teslas (T).
*
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = initialHardIronZ;
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Sets initial hard-iron bias coordinates of magnetometer used to find
* a solution expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
this.initialHardIronY = initialHardIronY;
this.initialHardIronZ = initialHardIronZ;
}
/**
* Sets initial hard iron coordinates of magnetometer used to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @return initial hard-iron.
*/
@Override
public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
initialHardIronX, initialHardIronY, initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial hard-iron used to find a solution.
*
* @param initialHardIron initial hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getInitialHardIron(result);
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getInitialHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialHardIronX;
result[1] = initialHardIronY;
result[2] = initialHardIronZ;
}
/**
* Sets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron[0];
initialHardIronY = initialHardIron[1];
initialHardIronZ = initialHardIron[2];
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @return initial hard-iron bias to be used to find a solution as a
* column matrix.
*/
@Override
public Matrix getInitialHardIronAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getInitialHardIronAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialHardIronAsMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialHardIronX);
result.setElementAtIndex(1, initialHardIronY);
result.setElementAtIndex(2, initialHardIronZ);
}
/**
* Sets initial hard-iron bias to be used to find a solution as a column
* matrix with values expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron.getElementAtIndex(0);
initialHardIronY = initialHardIron.getElementAtIndex(1);
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets a list of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 482 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1299 |
}
/**
* Gets x-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return x-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronX() {
return hardIronX;
}
/**
* Sets x-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronX x coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final double hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
}
/**
* Gets y-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return y-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronY() {
return hardIronY;
}
/**
* Sets y-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronY y coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final double hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = hardIronY;
}
/**
* Gets z-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return z-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronZ() {
return hardIronZ;
}
/**
* Sets z-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronZ z coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = hardIronZ;
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @return x coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known x-coordinate of magnetometer hard-iron.
*
* @param hardIronX known x-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @return y coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known y-coordinate of magnetometer hard-iron.
*
* @param hardIronY known y-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = convertMagneticFluxDensity(hardIronY);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @return z coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known z-coordinate of magnetometer hard-iron.
*
* @param hardIronZ known z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Sets known hard-iron bias coordinates of magnetometer expressed
* in Teslas (T).
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
this.hardIronY = hardIronY;
this.hardIronZ = hardIronZ;
}
/**
* Sets known hard-iron coordinates.
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
this.hardIronY = convertMagneticFluxDensity(hardIronY);
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Gets known hard-iron.
*
* @return known hard-iron.
*/
@Override
public MagneticFluxDensityTriad getHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
}
/**
* Gets known hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known hard-iron.
*
* @param hardIron hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getHardIron(result);
return result;
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = hardIronX;
result[1] = hardIronY;
result[2] = hardIronZ;
}
/**
* Sets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param hardIron known hard-iron bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setHardIron(final double[] hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
hardIronX = hardIron[0];
hardIronY = hardIron[1];
hardIronZ = hardIron[2];
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @return hard-iron bias as a column matrix.
*/
@Override
public Matrix getHardIronMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getHardIronMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getHardIronMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, hardIronX);
result.setElementAtIndex(1, hardIronY);
result.setElementAtIndex(2, hardIronZ);
}
/**
* Sets known hard-iron bias.
*
* @param hardIron magnetometer hard-iron bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setHardIron(final Matrix hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
hardIronX = hardIron.getElementAtIndex(0);
hardIronY = hardIron.getElementAtIndex(1);
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets a list of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1136 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1298 |
}
}
/**
* Gets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronX() {
return hardIronX;
}
/**
* Sets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronX known x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final double hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
}
/**
* Gets known y-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronY() {
return hardIronY;
}
/**
* Sets known y-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronY known y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final double hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = hardIronY;
}
/**
* Gets known z-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronZ() {
return hardIronZ;
}
/**
* Sets known z-coordinate of magnetometer hard-iron bias.
* This is expressed in meters Teslas (T).
*
* @param hardIronZ known z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = hardIronZ;
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @return x coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known x-coordinate of magnetometer hard-iron.
*
* @param hardIronX known x-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @return y coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known y-coordinate of magnetometer hard-iron.
*
* @param hardIronY known y-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = convertMagneticFluxDensity(hardIronY);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @return z coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known z-coordinate of magnetometer hard-iron.
*
* @param hardIronZ known z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Sets known hard-iron bias coordinates of magnetometer expressed in
* Teslas (T).
*
* @param hardIronX x-coordinate of magnetometer
* known hard-iron bias.
* @param hardIronY y-coordinate of magnetometer
* known hard-iron bias.
* @param hardIronZ z-coordinate of magnetometer
* known hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
this.hardIronY = hardIronY;
this.hardIronZ = hardIronZ;
}
/**
* Sets known hard-iron coordinates.
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
this.hardIronY = convertMagneticFluxDensity(hardIronY);
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Gets known hard-iron.
*
* @return known hard-iron.
*/
@Override
public MagneticFluxDensityTriad getHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
}
/**
* Gets known hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known hard-iron.
*
* @param hardIron hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getHardIron(result);
return result;
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = hardIronX;
result[1] = hardIronY;
result[2] = hardIronZ;
}
/**
* Sets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param hardIron known hard-iron.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setHardIron(final double[] hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
hardIronX = hardIron[0];
hardIronY = hardIron[1];
hardIronZ = hardIron[2];
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @return known hard-iron bias as a column matrix.
*/
@Override
public Matrix getHardIronMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getHardIronMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getHardIronMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, hardIronX);
result.setElementAtIndex(1, hardIronY);
result.setElementAtIndex(2, hardIronZ);
}
/**
* Sets known hard-iron bias as a column matrix.
*
* @param hardIron known hard-iron bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setHardIron(final Matrix hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
hardIronX = hardIron.getElementAtIndex(0);
hardIronY = hardIron.getElementAtIndex(1);
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1193 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1301 |
}
}
/**
* Gets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronX() {
return initialHardIronX;
}
/**
* Sets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
}
/**
* Gets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronY() {
return initialHardIronY;
}
/**
* Sets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = initialHardIronY;
}
/**
* Gets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronZ() {
return initialHardIronZ;
}
/**
* Sets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in meters Teslas (T).
*
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = initialHardIronZ;
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Sets initial hard-iron bias coordinates of magnetometer used to find
* a solution expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
this.initialHardIronY = initialHardIronY;
this.initialHardIronZ = initialHardIronZ;
}
/**
* Sets initial hard iron coordinates of magnetometer used to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @return initial hard-iron.
*/
@Override
public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
initialHardIronX, initialHardIronY, initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial hard-iron used to find a solution.
*
* @param initialHardIron initial hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getInitialHardIron(result);
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getInitialHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialHardIronX;
result[1] = initialHardIronY;
result[2] = initialHardIronZ;
}
/**
* Sets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron[0];
initialHardIronY = initialHardIron[1];
initialHardIronZ = initialHardIron[2];
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @return initial hard-iron bias to be used to find a solution as a
* column matrix.
*/
@Override
public Matrix getInitialHardIronAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getInitialHardIronAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialHardIronAsMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialHardIronX);
result.setElementAtIndex(1, initialHardIronY);
result.setElementAtIndex(2, initialHardIronZ);
}
/**
* Sets initial hard-iron bias to be used to find a solution as a column
* matrix with values expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron.getElementAtIndex(0);
initialHardIronY = initialHardIron.getElementAtIndex(1);
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1137 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 482 |
}
/**
* Gets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronX() {
return hardIronX;
}
/**
* Sets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronX known x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final double hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
}
/**
* Gets known y-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronY() {
return hardIronY;
}
/**
* Sets known y-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronY known y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final double hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = hardIronY;
}
/**
* Gets known z-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronZ() {
return hardIronZ;
}
/**
* Sets known z-coordinate of magnetometer hard-iron bias.
* This is expressed in meters Teslas (T).
*
* @param hardIronZ known z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = hardIronZ;
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @return x coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known x-coordinate of magnetometer hard-iron.
*
* @param hardIronX known x-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @return y coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known y-coordinate of magnetometer hard-iron.
*
* @param hardIronY known y-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = convertMagneticFluxDensity(hardIronY);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @return z coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known z-coordinate of magnetometer hard-iron.
*
* @param hardIronZ known z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Sets known hard-iron bias coordinates of magnetometer expressed in
* Teslas (T).
*
* @param hardIronX x-coordinate of magnetometer
* known hard-iron bias.
* @param hardIronY y-coordinate of magnetometer
* known hard-iron bias.
* @param hardIronZ z-coordinate of magnetometer
* known hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
this.hardIronY = hardIronY;
this.hardIronZ = hardIronZ;
}
/**
* Sets known hard-iron coordinates.
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
this.hardIronY = convertMagneticFluxDensity(hardIronY);
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Gets known hard-iron.
*
* @return known hard-iron.
*/
@Override
public MagneticFluxDensityTriad getHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
}
/**
* Gets known hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known hard-iron.
*
* @param hardIron hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getHardIron(result);
return result;
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = hardIronX;
result[1] = hardIronY;
result[2] = hardIronZ;
}
/**
* Sets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param hardIron known hard-iron.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setHardIron(final double[] hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
hardIronX = hardIron[0];
hardIronY = hardIron[1];
hardIronZ = hardIron[2];
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @return known hard-iron bias as a column matrix.
*/
@Override
public Matrix getHardIronMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getHardIronMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getHardIronMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, hardIronX);
result.setElementAtIndex(1, hardIronY);
result.setElementAtIndex(2, hardIronZ);
}
/**
* Sets known hard-iron bias as a column matrix.
*
* @param hardIron known hard-iron bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setHardIron(final Matrix hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
hardIronX = hardIron.getElementAtIndex(0);
hardIronY = hardIron.getElementAtIndex(1);
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1194 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 488 |
}
/**
* Gets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronX() {
return initialHardIronX;
}
/**
* Sets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
}
/**
* Gets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronY() {
return initialHardIronY;
}
/**
* Sets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = initialHardIronY;
}
/**
* Gets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronZ() {
return initialHardIronZ;
}
/**
* Sets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in meters Teslas (T).
*
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = initialHardIronZ;
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Sets initial hard-iron bias coordinates of magnetometer used to find
* a solution expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
this.initialHardIronY = initialHardIronY;
this.initialHardIronZ = initialHardIronZ;
}
/**
* Sets initial hard iron coordinates of magnetometer used to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @return initial hard-iron.
*/
@Override
public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
initialHardIronX, initialHardIronY, initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial hard-iron used to find a solution.
*
* @param initialHardIron initial hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getInitialHardIron(result);
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getInitialHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialHardIronX;
result[1] = initialHardIronY;
result[2] = initialHardIronZ;
}
/**
* Sets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron[0];
initialHardIronY = initialHardIron[1];
initialHardIronZ = initialHardIron[2];
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @return initial hard-iron bias to be used to find a solution as a
* column matrix.
*/
@Override
public Matrix getInitialHardIronAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getInitialHardIronAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialHardIronAsMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialHardIronX);
result.setElementAtIndex(1, initialHardIronY);
result.setElementAtIndex(2, initialHardIronZ);
}
/**
* Sets initial hard-iron bias to be used to find a solution as a column
* matrix with values expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron.getElementAtIndex(0);
initialHardIronY = initialHardIron.getElementAtIndex(1);
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2732 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1302 |
}
/**
* Gets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronX() {
return initialHardIronX;
}
/**
* Sets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
}
/**
* Gets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronY() {
return initialHardIronY;
}
/**
* Sets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = initialHardIronY;
}
/**
* Gets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronZ() {
return initialHardIronZ;
}
/**
* Sets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in meters Teslas (T).
*
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = initialHardIronZ;
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Sets initial hard-iron bias coordinates of magnetometer used to find
* a solution expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
this.initialHardIronY = initialHardIronY;
this.initialHardIronZ = initialHardIronZ;
}
/**
* Sets initial hard iron coordinates of magnetometer used to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @return initial hard-iron.
*/
@Override
public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
initialHardIronX, initialHardIronY, initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial hard-iron used to find a solution.
*
* @param initialHardIron initial hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getInitialHardIron(result);
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getInitialHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialHardIronX;
result[1] = initialHardIronY;
result[2] = initialHardIronZ;
}
/**
* Sets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron[0];
initialHardIronY = initialHardIron[1];
initialHardIronZ = initialHardIron[2];
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @return initial hard-iron bias to be used to find a solution as a
* column matrix.
*/
@Override
public Matrix getInitialHardIronAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getInitialHardIronAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialHardIronAsMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialHardIronX);
result.setElementAtIndex(1, initialHardIronY);
result.setElementAtIndex(2, initialHardIronZ);
}
/**
* Sets initial hard-iron bias to be used to find a solution as a column
* matrix with values expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron.getElementAtIndex(0);
initialHardIronY = initialHardIron.getElementAtIndex(1);
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2724 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1299 |
}
/**
* Gets x-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return x-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronX() {
return hardIronX;
}
/**
* Sets x-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronX x coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final double hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
}
/**
* Gets y-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return y-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronY() {
return hardIronY;
}
/**
* Sets y-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronY y coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final double hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = hardIronY;
}
/**
* Gets z-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return z-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronZ() {
return hardIronZ;
}
/**
* Sets z-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronZ z coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = hardIronZ;
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @return x coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known x-coordinate of magnetometer hard-iron.
*
* @param hardIronX known x-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @return y coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known y-coordinate of magnetometer hard-iron.
*
* @param hardIronY known y-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = convertMagneticFluxDensity(hardIronY);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @return z coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known z-coordinate of magnetometer hard-iron.
*
* @param hardIronZ known z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Sets known hard-iron bias coordinates of magnetometer expressed
* in Teslas (T).
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
this.hardIronY = hardIronY;
this.hardIronZ = hardIronZ;
}
/**
* Sets known hard-iron coordinates.
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
this.hardIronY = convertMagneticFluxDensity(hardIronY);
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Gets known hard-iron.
*
* @return known hard-iron.
*/
@Override
public MagneticFluxDensityTriad getHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
}
/**
* Gets known hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known hard-iron.
*
* @param hardIron hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getHardIron(result);
return result;
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = hardIronX;
result[1] = hardIronY;
result[2] = hardIronZ;
}
/**
* Sets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param hardIron known hard-iron bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setHardIron(final double[] hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
hardIronX = hardIron[0];
hardIronY = hardIron[1];
hardIronZ = hardIron[2];
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @return hard-iron bias as a column matrix.
*/
@Override
public Matrix getHardIronMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getHardIronMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getHardIronMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, hardIronX);
result.setElementAtIndex(1, hardIronY);
result.setElementAtIndex(2, hardIronZ);
}
/**
* Sets known hard-iron bias.
*
* @param hardIron magnetometer hard-iron bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setHardIron(final Matrix hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
hardIronX = hardIron.getElementAtIndex(0);
hardIronY = hardIron.getElementAtIndex(1);
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 3758 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 1824 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1118 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1074 |
}
/**
* Gets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() {
return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasXAsAcceleration(final Acceleration result) {
result.setValue(biasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets x-coordinate of known accelerometer bias.
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final Acceleration biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
}
/**
* Gets y-coordinate of known accelerometer bias.
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasYAsAcceleration() {
return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets y-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasYAsAcceleration(final Acceleration result) {
result.setValue(biasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets y-coordinate of known accelerometer bias.
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final Acceleration biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAcceleration(biasY);
}
/**
* Gets z-coordinate of known accelerometer bias.
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasZAsAcceleration() {
return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets z-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasZAsAcceleration(final Acceleration result) {
result.setValue(biasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets z-coordinate of known accelerometer bias to be used to find a solution.
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAcceleration(biasZ);
}
/**
* Sets known bias coordinates of accelerometer expressed in meters
* per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @param biasY y-coordinate of known accelerometer bias.
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known bias coordinates of accelerometer.
*
* @param biasX x-coordinate of known accelerometer bias.
* @param biasY y-coordinate of known accelerometer bias.
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(
final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
this.biasY = convertAcceleration(biasY);
this.biasZ = convertAcceleration(biasZ);
}
/**
* Gets known accelerometer bias.
*
* @return known accelerometer bias.
*/
@Override
public AccelerationTriad getBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known accelerometer bias.
*
* @param result instance where result will be stored.
*/
@Override
public void getBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known accelerometer bias.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBias(final AccelerationTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known bias to be set
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known bias as a column matrix.
*
* @return known bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known bias as an array.
*
* @param bias bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1136 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1093 |
this(convertAcceleration(groundTruthGravityNorm), measurements, commonAxisUsed, initialBias, initialMa,
listener);
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solutions.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasXAsAcceleration() {
return new Acceleration(initialBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasXAsAcceleration(final Acceleration result) {
result.setValue(initialBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final Acceleration initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAcceleration(initialBiasX);
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasYAsAcceleration() {
return new Acceleration(initialBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasYAsAcceleration(final Acceleration result) {
result.setValue(initialBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final Acceleration initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAcceleration(initialBiasY);
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasZAsAcceleration() {
return new Acceleration(initialBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasZAsAcceleration(final Acceleration result) {
result.setValue(initialBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final Acceleration initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAcceleration(initialBiasZ);
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution
* expressed in meters per squared second (m/s^2).
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final double initialBiasX, final double initialBiasY, final double initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(
final Acceleration initialBiasX, final Acceleration initialBiasY, final Acceleration initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAcceleration(initialBiasX);
this.initialBiasY = convertAcceleration(initialBiasY);
this.initialBiasZ = convertAcceleration(initialBiasZ);
}
/**
* Gets initial bias coordinates of accelerometer used to find a solution.
*
* @return initial bias coordinates.
*/
@Override
public AccelerationTriad getInitialBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of accelerometer used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution.
*
* @param initialBias initial bias coordinates to be set.
*/
@Override
public void setInitialBias(final AccelerationTriad initialBias) {
initialBiasX = convertAcceleration(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAcceleration(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial x scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* This is only taken into account if non-linear preliminary solutions are used.
* Values are expressed in meters per squared second (m/s^2).
*
* @return initial bias to be used to find a solution as a column matrix.
*/
@Override
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* This is only taken into account if non-linear preliminary solutions are used.
* Values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial bias to be used to find a solution as an array.
* This is only taken into account if non-linear preliminary solutions are used.
* Values are expressed in meters per squared second (m/s^2).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets ground truth gravity norm to be expected at location where measurements have been made,
* expressed in meter per squared second (m/s^2).
*
* @return ground truth gravity norm or null.
*/
public Double getGroundTruthGravityNorm() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1137 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2724 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 482 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1299 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1264 |
}
/**
* Gets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronX() {
return hardIronX;
}
/**
* Sets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronX known x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final double hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
}
/**
* Gets known y-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronY() {
return hardIronY;
}
/**
* Sets known y-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronY known y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final double hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = hardIronY;
}
/**
* Gets known z-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronZ() {
return hardIronZ;
}
/**
* Sets known z-coordinate of magnetometer hard-iron bias.
* This is expressed in meters Teslas (T).
*
* @param hardIronZ known z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = hardIronZ;
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @return x coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known x-coordinate of magnetometer hard-iron.
*
* @param hardIronX known x-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @return y coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known y-coordinate of magnetometer hard-iron.
*
* @param hardIronY known y-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = convertMagneticFluxDensity(hardIronY);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @return z coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known z-coordinate of magnetometer hard-iron.
*
* @param hardIronZ known z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Sets known hard-iron bias coordinates of magnetometer expressed in
* Teslas (T).
*
* @param hardIronX x-coordinate of magnetometer
* known hard-iron bias.
* @param hardIronY y-coordinate of magnetometer
* known hard-iron bias.
* @param hardIronZ z-coordinate of magnetometer
* known hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
this.hardIronY = hardIronY;
this.hardIronZ = hardIronZ;
}
/**
* Sets known hard-iron coordinates.
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
this.hardIronY = convertMagneticFluxDensity(hardIronY);
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Gets known hard-iron.
*
* @return known hard-iron.
*/
@Override
public MagneticFluxDensityTriad getHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
}
/**
* Gets known hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known hard-iron.
*
* @param hardIron hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getHardIron(result);
return result;
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = hardIronX;
result[1] = hardIronY;
result[2] = hardIronZ;
}
/**
* Sets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param hardIron known hard-iron.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setHardIron(final double[] hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
hardIronX = hardIron[0];
hardIronY = hardIron[1];
hardIronZ = hardIron[2];
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @return known hard-iron bias as a column matrix.
*/
@Override
public Matrix getHardIronMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getHardIronMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getHardIronMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, hardIronX);
result.setElementAtIndex(1, hardIronY);
result.setElementAtIndex(2, hardIronZ);
}
/**
* Sets known hard-iron bias as a column matrix.
*
* @param hardIron known hard-iron bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setHardIron(final Matrix hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
hardIronX = hardIron.getElementAtIndex(0);
hardIronY = hardIron.getElementAtIndex(1);
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1194 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2732 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 488 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1302 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 1267 |
}
/**
* Gets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronX() {
return initialHardIronX;
}
/**
* Sets initial x-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final double initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
}
/**
* Gets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronY() {
return initialHardIronY;
}
/**
* Sets initial y-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final double initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = initialHardIronY;
}
/**
* Gets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in Teslas (T).
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getInitialHardIronZ() {
return initialHardIronZ;
}
/**
* Sets initial z-coordinate of magnetometer hard-iron bias to be used
* to find a solution.
* This is expressed in meters Teslas (T).
*
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final double initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = initialHardIronZ;
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial x-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial x-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronX(final MagneticFluxDensity initialHardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial y-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial y-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronY(final MagneticFluxDensity initialHardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @return initial z-coordinate of magnetometer hard-iron bias.
*/
@Override
public MagneticFluxDensity getInitialHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(initialHardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(initialHardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial z-coordinate of magnetometer hard iron bias to be used
* to find a solution.
*
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIronZ(final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Sets initial hard-iron bias coordinates of magnetometer used to find
* a solution expressed in Teslas (T).
*
* @param initialHardIronX initial x-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronY initial y-coordinate of magnetometer
* hard-iron bias.
* @param initialHardIronZ initial z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final double initialHardIronX, final double initialHardIronY, final double initialHardIronZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = initialHardIronX;
this.initialHardIronY = initialHardIronY;
this.initialHardIronZ = initialHardIronZ;
}
/**
* Sets initial hard iron coordinates of magnetometer used to find a solution.
*
* @param initialHardIronX initial x-coordinate of magnetometer bias.
* @param initialHardIronY initial y-coordinate of magnetometer bias.
* @param initialHardIronZ initial z-coordinate of magnetometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(
final MagneticFluxDensity initialHardIronX, final MagneticFluxDensity initialHardIronY,
final MagneticFluxDensity initialHardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialHardIronX = convertMagneticFluxDensity(initialHardIronX);
this.initialHardIronY = convertMagneticFluxDensity(initialHardIronY);
this.initialHardIronZ = convertMagneticFluxDensity(initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @return initial hard-iron.
*/
@Override
public MagneticFluxDensityTriad getInitialHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
initialHardIronX, initialHardIronY, initialHardIronZ);
}
/**
* Gets initial hard-iron used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(initialHardIronX, initialHardIronY, initialHardIronZ,
MagneticFluxDensityUnit.TESLA);
}
/**
* Sets initial hard-iron used to find a solution.
*
* @param initialHardIron initial hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialHardIron(final MagneticFluxDensityTriad initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
initialHardIronX = convertMagneticFluxDensity(initialHardIron.getValueX(), initialHardIron.getUnit());
initialHardIronY = convertMagneticFluxDensity(initialHardIron.getValueY(), initialHardIron.getUnit());
initialHardIronZ = convertMagneticFluxDensity(initialHardIron.getValueZ(), initialHardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getInitialHardIron(result);
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getInitialHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialHardIronX;
result[1] = initialHardIronY;
result[2] = initialHardIronZ;
}
/**
* Sets initial hard-iron bias to be used to find a solution as an array.
* Array values are expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialHardIron(final double[] initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron[0];
initialHardIronY = initialHardIron[1];
initialHardIronZ = initialHardIron[2];
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @return initial hard-iron bias to be used to find a solution as a
* column matrix.
*/
@Override
public Matrix getInitialHardIronAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getInitialHardIronAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial hard-iron bias to be used to find a solution as a
* column matrix.
* Values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialHardIronAsMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialHardIronX);
result.setElementAtIndex(1, initialHardIronY);
result.setElementAtIndex(2, initialHardIronZ);
}
/**
* Sets initial hard-iron bias to be used to find a solution as a column
* matrix with values expressed in Teslas (T).
*
* @param initialHardIron initial hard-iron bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialHardIron(final Matrix initialHardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialHardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || initialHardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialHardIronX = initialHardIron.getElementAtIndex(0);
initialHardIronY = initialHardIron.getElementAtIndex(1);
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 471 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1138 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1094 |
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solutions.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasXAsAcceleration() {
return new Acceleration(initialBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasXAsAcceleration(final Acceleration result) {
result.setValue(initialBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final Acceleration initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAcceleration(initialBiasX);
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasYAsAcceleration() {
return new Acceleration(initialBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasYAsAcceleration(final Acceleration result) {
result.setValue(initialBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final Acceleration initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAcceleration(initialBiasY);
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasZAsAcceleration() {
return new Acceleration(initialBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasZAsAcceleration(final Acceleration result) {
result.setValue(initialBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final Acceleration initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAcceleration(initialBiasZ);
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution
* expressed in meters per squared second (m/s^2).
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final double initialBiasX, final double initialBiasY, final double initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final Acceleration initialBiasX, final Acceleration initialBiasY,
final Acceleration initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAcceleration(initialBiasX);
this.initialBiasY = convertAcceleration(initialBiasY);
this.initialBiasZ = convertAcceleration(initialBiasZ);
}
/**
* Gets initial bias coordinates of accelerometer used to find a solution.
*
* @return initial bias coordinates.
*/
@Override
public AccelerationTriad getInitialBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of accelerometer used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution.
*
* @param initialBias initial bias coordinates to be set.
*/
@Override
public void setInitialBias(final AccelerationTriad initialBias) {
initialBiasX = convertAcceleration(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAcceleration(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial x scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* This is only taken into account if non-linear preliminary solutions are used.
* Values are expressed in meters per squared second (m/s^2).
*
* @return initial bias to be used to find a solution as a column matrix.
*/
@Override
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* This is only taken into account if non-linear preliminary solutions are used.
* Values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial bias to be used to find a solution as an array.
* This is only taken into account if non-linear preliminary solutions are used.
* Values are expressed in meters per squared second (m/s^2).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2100 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2059 |
&& groundTruthGravityNorm != null;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFy() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFz() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFxAsAcceleration() {
return estimatedBiases != null
? new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFyAsAcceleration() {
return estimatedBiases != null
? new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFzAsAcceleration() {
return estimatedBiases != null
? new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer bias.
*
* @return estimated accelerometer bias or null if not available.
*/
@Override
public AccelerationTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated accelerometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets estimated covariance matrix for estimated calibration solution.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
* This is only available when result has been refined and covariance
* is kept.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets variance of estimated x coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated x coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFxVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated x coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFxStandardDeviation() {
final var variance = getEstimatedBiasFxVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias.
*
* @return standard deviation of estimated x coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFxStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFxStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND) :
null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFxStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFxStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated y coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFyVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated y coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFyStandardDeviation() {
final var variance = getEstimatedBiasFyVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias.
*
* @return standard deviation of estimated y coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFyStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFyStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFyStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFyStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated z coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFzVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated z coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFzStandardDeviation() {
final var variance = getEstimatedBiasFzVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias.
*
* @return standard deviation of estimated z coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFzStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFzStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFzStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFzStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated accelerometer bias coordinates.
*
* @return standard deviation of estimated accelerometer bias coordinates.
*/
public AccelerationTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null
? new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
getEstimatedBiasFxStandardDeviation(),
getEstimatedBiasFyStandardDeviation(),
getEstimatedBiasFzStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated accelerometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of accelerometer bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AccelerationTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasFxStandardDeviation(),
getEstimatedBiasFyStandardDeviation(),
getEstimatedBiasFzStandardDeviation(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates expressed
* in meters per squared second (m/s^2).
*
* @return average of estimated standard deviation of accelerometer bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null ? (getEstimatedBiasFxStandardDeviation()
+ getEstimatedBiasFyStandardDeviation() + getEstimatedBiasFzStandardDeviation()) / 3.0 : null;
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates.
*
* @return average of estimated standard deviation of accelerometer bias coordinates or null.
*/
public Acceleration getEstimatedBiasStandardDeviationAverageAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasStandardDeviationAverage(),
AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of accelerometer bias expressed in
* meters per squared second (m/s^2).
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of accelerometer bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasFxVariance() + getEstimatedBiasFyVariance() + getEstimatedBiasFzVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of accelerometer bias.
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of accelerometer bias or null
* if not available.
*/
public Acceleration getEstimatedBiasStandardDeviationNormAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasStandardDeviationNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of accelerometer bias coordinates.
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of accelerometer bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #getMinimumRequiredMeasurements()}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #getMinimumRequiredMeasurements()}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than {@link #getMinimumRequiredMeasurements()}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < getMinimumRequiredMeasurements()) {
throw new IllegalArgumentException();
}
this.preliminarySubsetSize = preliminarySubsetSize;
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
public abstract RobustEstimatorMethod getMethod();
/**
* Creates a robust accelerometer calibrator.
*
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2200 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2333 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @return array containing x,y,z components of estimated magnetometer
* hard-iron biases.
*/
@Override
public double[] getEstimatedHardIron() {
return estimatedHardIron;
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @param result instance where estimated magnetometer biases will be
* stored.
* @return true if result instance was updated, false otherwise (when
* estimation is not yet available).
*/
@Override
public boolean getEstimatedHardIron(final double[] result) {
if (estimatedHardIron != null) {
System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @return column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases.
*/
@Override
public Matrix getEstimatedHardIronAsMatrix() {
return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedHardIron != null) {
result.fromArray(estimatedHardIron);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return x coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronX() {
return estimatedHardIron != null ? estimatedHardIron[0] : null;
}
/**
* Gets y coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return y coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronY() {
return estimatedHardIron != null ? estimatedHardIron[1] : null;
}
/**
* Gets z coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return z coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronZ() {
return estimatedHardIron != null ? estimatedHardIron[2] : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @return x coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[0]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @return y coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[1]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @return z coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[2]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer bias.
*
* @return estimated magnetometer bias or null if not available.
*/
@Override
public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
return estimatedHardIron != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2])
: null;
}
/**
* Gets estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
if (estimatedHardIron != null) {
result.setValueCoordinatesAndUnit(estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2],
MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets variance of estimated x coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated x coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated x coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronXStandardDeviation() {
final var variance = getEstimatedHardIronXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias.
*
* @return standard deviation of estimated x coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronXStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronXStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated y coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated y coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronYStandardDeviation() {
final var variance = getEstimatedHardIronYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias.
*
* @return standard deviation of estimated y coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronYStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronYStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated z coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated z coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronZStandardDeviation() {
final var variance = getEstimatedHardIronZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias.
*
* @return standard deviation of estimated z coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronZStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronZStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated magnetometer bias coordinates.
*
* @return standard deviation of estimated magnetometer bias coordinates.
*/
public MagneticFluxDensityTriad getEstimatedHardIronStandardDeviation() {
return estimatedCovariance != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
getEstimatedHardIronXStandardDeviation(),
getEstimatedHardIronYStandardDeviation(),
getEstimatedHardIronZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated magnetometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of magnetometer bias was available,
* false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviation(final MagneticFluxDensityTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedHardIronXStandardDeviation(),
getEstimatedHardIronYStandardDeviation(),
getEstimatedHardIronZStandardDeviation(),
MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates
* expressed in Teslas (T).
*
* @return average of estimated standard deviation of magnetometer bias coordinates,
* or null if not available.
*/
public Double getEstimatedHardIronStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedHardIronXStandardDeviation() + getEstimatedHardIronYStandardDeviation()
+ getEstimatedHardIronZStandardDeviation()) / 3.0
: null;
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates.
*
* @return average of estimated standard deviation of magnetometer bias coordinates,
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationAverage(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of magnetometer bias is available,
* false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronStandardDeviationAverage());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of magnetometer bias expressed in
* Teslas (T).
*
* @return norm of estimated standard deviation of magnetometer bias or null
* if not available.
*/
public Double getEstimatedHardIronStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedHardIronXVariance() + getEstimatedHardIronYVariance()
+ getEstimatedHardIronZVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of magnetometer bias.
*
* @return norm of estimated standard deviation of magnetometer bias or null
* if not available.
*/
public MagneticFluxDensity getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationNorm(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets norm of estimated standard deviation of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of magnetometer bias
* is available, false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronStandardDeviationNorm());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
throw new IllegalArgumentException();
}
this.preliminarySubsetSize = preliminarySubsetSize;
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
public abstract RobustEstimatorMethod getMethod();
/**
* Creates a robust magnetometer calibrator.
*
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1514 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4065 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where estimated gyroscope biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return column matrix containing x,y,z components of estimated gyroscope
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasX() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasY() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasZ() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedX() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedY() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedZ() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope bias.
*
* @return estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeedTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if estimated gyroscope bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated covariance matrix for estimated calibration solution.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
* This is only available when result has been refined and covariance
* is kept.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated x coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasXStandardDeviation() {
final var variance = getEstimatedBiasXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasXStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated y coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasYStandardDeviation() {
final var variance = getEstimatedBiasYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasYStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated z coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasZStandardDeviation() {
final var variance = getEstimatedBiasZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasZStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @return standard deviation of estimated gyroscope bias coordinates.
*/
public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of gyroscope bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation(),
AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates expressed
* in radians per second (rad/s).
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
+ getEstimatedBiasZStandardDeviation()) / 3.0 : null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null.
*/
public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of gyroscope bias expressed in
* radians per second (rad/s).
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias coordinates.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of gyroscope bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize)
throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1453 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2111 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2070 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFy() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFz() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFxAsAcceleration() {
return estimatedBiases != null
? new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFyAsAcceleration() {
return estimatedBiases != null
? new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFzAsAcceleration() {
return estimatedBiases != null
? new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer bias.
*
* @return estimated accelerometer bias or null if not available.
*/
@Override
public AccelerationTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated accelerometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets estimated covariance matrix for estimated calibration solution.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
* This is only available when result has been refined and covariance
* is kept.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets variance of estimated x coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated x coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFxVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated x coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFxStandardDeviation() {
final var variance = getEstimatedBiasFxVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias.
*
* @return standard deviation of estimated x coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFxStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFxStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFxStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFxStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated y coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFyVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated y coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFyStandardDeviation() {
final var variance = getEstimatedBiasFyVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias.
*
* @return standard deviation of estimated y coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFyStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFyStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFyStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFyStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated z coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFzVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated z coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFzStandardDeviation() {
final var variance = getEstimatedBiasFzVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias.
*
* @return standard deviation of estimated z coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFzStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFzStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFzStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFzStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated accelerometer bias coordinates.
*
* @return standard deviation of estimated accelerometer bias coordinates.
*/
public AccelerationTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null
? new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
getEstimatedBiasFxStandardDeviation(),
getEstimatedBiasFyStandardDeviation(),
getEstimatedBiasFzStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated accelerometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of accelerometer bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AccelerationTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasFxStandardDeviation(),
getEstimatedBiasFyStandardDeviation(),
getEstimatedBiasFzStandardDeviation(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates expressed
* in meters per squared second (m/s^2).
*
* @return average of estimated standard deviation of accelerometer bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedBiasFxStandardDeviation() + getEstimatedBiasFyStandardDeviation()
+ getEstimatedBiasFzStandardDeviation()) / 3.0
: null;
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates.
*
* @return average of estimated standard deviation of accelerometer bias coordinates or null.
*/
public Acceleration getEstimatedBiasStandardDeviationAverageAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasStandardDeviationAverage(),
AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of accelerometer bias expressed in
* meters per squared second (m/s^2).
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of accelerometer bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasFxVariance() + getEstimatedBiasFyVariance() + getEstimatedBiasFzVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of accelerometer bias.
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of accelerometer bias or null
* if not available.
*/
public Acceleration getEstimatedBiasStandardDeviationNormAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasStandardDeviationNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of accelerometer bias coordinates.
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of accelerometer bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
*/
public void setPreliminarySubsetSize(
final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1479 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2200 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2333 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @return array containing x,y,z components of estimated magnetometer
* hard-iron biases.
*/
@Override
public double[] getEstimatedHardIron() {
return estimatedHardIron;
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @param result instance where estimated magnetometer biases will be
* stored.
* @return true if result instance was updated, false otherwise (when
* estimation is not yet available).
*/
@Override
public boolean getEstimatedHardIron(final double[] result) {
if (estimatedHardIron != null) {
System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @return column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases.
*/
@Override
public Matrix getEstimatedHardIronAsMatrix() {
return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedHardIron != null) {
result.fromArray(estimatedHardIron);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return x coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronX() {
return estimatedHardIron != null ? estimatedHardIron[0] : null;
}
/**
* Gets y coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return y coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronY() {
return estimatedHardIron != null ? estimatedHardIron[1] : null;
}
/**
* Gets z coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return z coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronZ() {
return estimatedHardIron != null ? estimatedHardIron[2] : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @return x coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[0]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @return y coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[1]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @return z coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[2]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer bias.
*
* @return estimated magnetometer bias or null if not available.
*/
@Override
public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
return estimatedHardIron != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2]) : null;
}
/**
* Gets estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
if (estimatedHardIron != null) {
result.setValueCoordinatesAndUnit(
estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2], MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets variance of estimated x coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated x coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated x coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronXStandardDeviation() {
final var variance = getEstimatedHardIronXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias.
*
* @return standard deviation of estimated x coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronXStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronXStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated y coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated y coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronYStandardDeviation() {
final var variance = getEstimatedHardIronYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias.
*
* @return standard deviation of estimated y coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronYStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronYStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated z coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated z coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronZStandardDeviation() {
final var variance = getEstimatedHardIronZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias.
*
* @return standard deviation of estimated z coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronZStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronZStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated magnetometer bias coordinates.
*
* @return standard deviation of estimated magnetometer bias coordinates.
*/
public MagneticFluxDensityTriad getEstimatedHardIronStandardDeviation() {
return estimatedCovariance != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
getEstimatedHardIronXStandardDeviation(),
getEstimatedHardIronYStandardDeviation(),
getEstimatedHardIronZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated magnetometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of magnetometer bias was available,
* false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviation(final MagneticFluxDensityTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedHardIronXStandardDeviation(),
getEstimatedHardIronYStandardDeviation(),
getEstimatedHardIronZStandardDeviation(),
MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates
* expressed in Teslas (T).
*
* @return average of estimated standard deviation of magnetometer bias coordinates,
* or null if not available.
*/
public Double getEstimatedHardIronStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedHardIronXStandardDeviation() + getEstimatedHardIronYStandardDeviation()
+ getEstimatedHardIronZStandardDeviation()) / 3.0
: null;
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates.
*
* @return average of estimated standard deviation of magnetometer bias coordinates,
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationAverage(),
MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of magnetometer bias is available,
* false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity(
final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronStandardDeviationAverage());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of magnetometer bias expressed in
* Teslas (T).
*
* @return norm of estimated standard deviation of magnetometer bias or null
* if not available.
*/
public Double getEstimatedHardIronStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedHardIronXVariance()
+ getEstimatedHardIronYVariance()
+ getEstimatedHardIronZVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of magnetometer bias.
*
* @return norm of estimated standard deviation of magnetometer bias or null
* if not available.
*/
public MagneticFluxDensity getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationNorm(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets norm of estimated standard deviation of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of magnetometer bias
* is available, false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronStandardDeviationNorm());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 1049 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 1019 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 1048 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 1022 |
this(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
this.listener = listener;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX,
final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy,
final double accelerometerSz, final double accelerometerMxy,
final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx,
final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 2101 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 2065 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 2113 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 2152 |
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 1052 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 2103 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 1051 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 2067 |
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX,
final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz,
final double accelerometerMyx, final double accelerometerMyz,
final double accelerometerMzx, final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy,
final double accelerometerSz, final double accelerometerMxy,
final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx,
final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx,
accelerometerMyz, accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 1022 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 1025 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 2115 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 2154 |
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasX() {
return accelerometerBiasX;
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX known x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final double accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasY() {
return accelerometerBiasY;
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasY known y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final double accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = accelerometerBiasY;
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public double getAccelerometerBiasZ() {
return accelerometerBiasZ;
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasZ known z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final double accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasXAsAcceleration() {
return new Acceleration(accelerometerBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasX(final Acceleration accelerometerBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasYAsAcceleration() {
return new Acceleration(accelerometerBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasY(final Acceleration accelerometerBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getAccelerometerBiasZAsAcceleration() {
return new Acceleration(accelerometerBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param result instance where result data will be stored.
*/
@Override
public void getAccelerometerBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerometerBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z-coordinate of accelerometer bias to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBiasZ(final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final double accelerometerBiasX, final double accelerometerBiasY, final double accelerometerBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = accelerometerBiasX;
this.accelerometerBiasY = accelerometerBiasY;
this.accelerometerBiasZ = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
*
* @param accelerometerBiasX x-coordinate of accelerometer bias.
* @param accelerometerBiasY y-coordinate of accelerometer bias.
* @param accelerometerBiasZ z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerBias(
final Acceleration accelerometerBiasX, final Acceleration accelerometerBiasY,
final Acceleration accelerometerBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerBiasX = convertAcceleration(accelerometerBiasX);
this.accelerometerBiasY = convertAcceleration(accelerometerBiasY);
this.accelerometerBiasZ = convertAcceleration(accelerometerBiasZ);
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public double[] getAccelerometerBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getAccelerometerBias(result);
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getAccelerometerBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = accelerometerBiasX;
result[1] = accelerometerBiasY;
result[2] = accelerometerBiasZ;
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void setAccelerometerBias(final double[] accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias[0];
accelerometerBiasY = accelerometerBias[1];
accelerometerBiasZ = accelerometerBias[2];
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @return known accelerometer bias.
*/
@Override
public Matrix getAccelerometerBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getAccelerometerBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getAccelerometerBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerBiasX);
result.setElementAtIndex(1, accelerometerBiasY);
result.setElementAtIndex(2, accelerometerBiasZ);
}
/**
* Sets known accelerometer bias to be used to fix measured specific
* force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*
* @param accelerometerBias known accelerometer bias. Must be 3x1.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setAccelerometerBias(final Matrix accelerometerBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerBias.getRows() != BodyKinematics.COMPONENTS || accelerometerBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
accelerometerBiasX = accelerometerBias.getElementAtIndex(0);
accelerometerBiasY = accelerometerBias.getElementAtIndex(1);
accelerometerBiasZ = accelerometerBias.getElementAtIndex(2);
}
/**
* Gets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer x scaling factor.
*/
@Override
public double getAccelerometerSx() {
return accelerometerSx;
}
/**
* Sets known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSx(final double accelerometerSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
}
/**
* Gets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer y scaling factor.
*/
@Override
public double getAccelerometerSy() {
return accelerometerSy;
}
/**
* Sets known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSy known accelerometer y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSy(final double accelerometerSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSy = accelerometerSy;
}
/**
* Gets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @return known accelerometer z scaling factor.
*/
@Override
public double getAccelerometerSz() {
return accelerometerSz;
}
/**
* Sets known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerSz(final double accelerometerSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSz = accelerometerSz;
}
/**
* Gets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-y cross coupling error.
*/
@Override
public double getAccelerometerMxy() {
return accelerometerMxy;
}
/**
* Sets known accelerometer x-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxy(final double accelerometerMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
}
/**
* Gets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer x-z cross coupling error.
*/
@Override
public double getAccelerometerMxz() {
return accelerometerMxz;
}
/**
* Sets known accelerometer x-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxz known accelerometer x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMxz(final double accelerometerMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxz = accelerometerMxz;
}
/**
* Gets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-x cross coupling error.
*/
@Override
public double getAccelerometerMyx() {
return accelerometerMyx;
}
/**
* Sets known accelerometer y-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyx(final double accelerometerMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyx = accelerometerMyx;
}
/**
* Gets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer y-z cross coupling error.
*/
@Override
public double getAccelerometerMyz() {
return accelerometerMyz;
}
/**
* Sets known accelerometer y-z cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMyz(final double accelerometerMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMyz = accelerometerMyz;
}
/**
* Gets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-x cross coupling error.
*/
@Override
public double getAccelerometerMzx() {
return accelerometerMzx;
}
/**
* Sets known accelerometer z-x cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzx(final double accelerometerMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzx = accelerometerMzx;
}
/**
* Gets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @return known accelerometer z-y cross coupling error.
*/
@Override
public double getAccelerometerMzy() {
return accelerometerMzy;
}
/**
* Sets known accelerometer z-y cross coupling error to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerMzy(final double accelerometerMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors to be used to fix measured
* specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerSx = accelerometerSx;
this.accelerometerSy = accelerometerSy;
this.accelerometerSz = accelerometerSz;
}
/**
* Sets known accelerometer cross coupling errors to be used to fix
* measured specific force and find cross biases introduced by the
* accelerometer.
*
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerCrossCouplingErrors(
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
this.accelerometerMxy = accelerometerMxy;
this.accelerometerMxz = accelerometerMxz;
this.accelerometerMyx = accelerometerMyx;
this.accelerometerMyz = accelerometerMyz;
this.accelerometerMzx = accelerometerMzx;
this.accelerometerMzy = accelerometerMzy;
}
/**
* Sets known accelerometer scaling factors and cross coupling errors
* to be used to fix measured specific force and find cross biases
* introduced by the accelerometer.
*
* @param accelerometerSx known accelerometer x scaling factor.
* @param accelerometerSy known accelerometer y scaling factor.
* @param accelerometerSz known accelerometer z scaling factor.
* @param accelerometerMxy known accelerometer x-y cross coupling
* error.
* @param accelerometerMxz known accelerometer x-z cross coupling
* error.
* @param accelerometerMyx known accelerometer y-x cross coupling
* error.
* @param accelerometerMyz known accelerometer y-z cross coupling
* error.
* @param accelerometerMzx known accelerometer z-x cross coupling
* error.
* @param accelerometerMzy known accelerometer z-y cross coupling
* error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setAccelerometerScalingFactorsAndCrossCouplingErrors(
final double accelerometerSx, final double accelerometerSy, final double accelerometerSz,
final double accelerometerMxy, final double accelerometerMxz, final double accelerometerMyx,
final double accelerometerMyz, final double accelerometerMzx, final double accelerometerMzy)
throws LockedException {
if (running) {
throw new LockedException();
}
setAccelerometerScalingFactors(accelerometerSx, accelerometerSy, accelerometerSz);
setAccelerometerCrossCouplingErrors(accelerometerMxy, accelerometerMxz, accelerometerMyx, accelerometerMyz,
accelerometerMzx, accelerometerMzy);
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @return known accelerometer scale factors and cross coupling
* errors matrix.
*/
@Override
public Matrix getAccelerometerMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getAccelerometerMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getAccelerometerMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, accelerometerSx);
result.setElementAtIndex(1, accelerometerMyx);
result.setElementAtIndex(2, accelerometerMzx);
result.setElementAtIndex(3, accelerometerMxy);
result.setElementAtIndex(4, accelerometerSy);
result.setElementAtIndex(5, accelerometerMzy);
result.setElementAtIndex(6, accelerometerMxz);
result.setElementAtIndex(7, accelerometerMyz);
result.setElementAtIndex(8, accelerometerSz);
}
/**
* Sets known accelerometer scale factors and cross coupling
* errors matrix.
*
* @param accelerometerMa known accelerometer scale factors and
* cross coupling errors matrix. Must be 3x3.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setAccelerometerMa(final Matrix accelerometerMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (accelerometerMa.getRows() != BodyKinematics.COMPONENTS
|| accelerometerMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
accelerometerSx = accelerometerMa.getElementAtIndex(0);
accelerometerMyx = accelerometerMa.getElementAtIndex(1);
accelerometerMzx = accelerometerMa.getElementAtIndex(2);
accelerometerMxy = accelerometerMa.getElementAtIndex(3);
accelerometerSy = accelerometerMa.getElementAtIndex(4);
accelerometerMzy = accelerometerMa.getElementAtIndex(5);
accelerometerMxz = accelerometerMa.getElementAtIndex(6);
accelerometerMyz = accelerometerMa.getElementAtIndex(7);
accelerometerSz = accelerometerMa.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2865 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4157 |
} catch (final FittingException | AlgebraException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where estimated gyroscope biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return column matrix containing x,y,z components of estimated gyroscope
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasX() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasY() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasZ() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedX() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedY() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedZ() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope bias.
*
* @return estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeedTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if estimated gyroscope bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated x coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasXStandardDeviation() {
final var variance = getEstimatedBiasXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasXStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated y coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasYStandardDeviation() {
final var variance = getEstimatedBiasYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasYStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated z coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasZStandardDeviation() {
final var variance = getEstimatedBiasZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasZStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @return standard deviation of estimated gyroscope bias coordinates.
*/
public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of gyroscope bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation(),
AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates expressed
* in radians per second (rad/s).
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
+ getEstimatedBiasZStandardDeviation()) / 3.0 : null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null.
*/
public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of gyroscope bias expressed in
* radians per second (rad/s).
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias coordinates.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of gyroscope bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Internal method to perform calibration when common z-axis is assumed
* for both the accelerometer and gyroscope and when G-dependent cross
* biases are being estimated.
*
* @throws AlgebraException if accelerometer parameters prevent fixing
* measured accelerometer values due to numerical instabilities.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateCommonAxisAndGDependentCrossBiases() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2865 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3225 |
} catch (final FittingException | AlgebraException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where estimated gyroscope biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return column matrix containing x,y,z components of estimated gyroscope
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasX() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasY() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasZ() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedX() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedY() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedZ() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope bias.
*
* @return estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeedTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if estimated gyroscope bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated x coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasXStandardDeviation() {
final var variance = getEstimatedBiasXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasXStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated y coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasYStandardDeviation() {
final var variance = getEstimatedBiasYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasYStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated z coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasZStandardDeviation() {
final var variance = getEstimatedBiasZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasZStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @return standard deviation of estimated gyroscope bias coordinates.
*/
public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of gyroscope bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation(),
AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates expressed
* in radians per second (rad/s).
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
+ getEstimatedBiasZStandardDeviation()) / 3.0 : null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null.
*/
public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of gyroscope bias expressed in
* radians per second (rad/s).
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias coordinates.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of gyroscope bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Internal method to perform calibration when common z-axis is assumed
* for both the accelerometer and gyroscope and when G-dependent cross
* biases are being estimated.
*
* @throws AlgebraException if accelerometer parameters prevent fixing
* measured accelerometer values due to numerical instabilities.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateCommonAxisAndGDependentCrossBiases() throws AlgebraException, FittingException, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4999 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2974 |
listener.onCalibrateEnd((C) this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFy() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFz() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFxAsAcceleration() {
return estimatedBiases != null ?
new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFyAsAcceleration() {
return estimatedBiases != null ?
new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFzAsAcceleration() {
return estimatedBiases != null ?
new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer bias.
*
* @return estimated accelerometer bias or null if not available.
*/
@Override
public AccelerationTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null ?
new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2]) : null;
}
/**
* Gets estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated accelerometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets variance of estimated x coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated x coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFxVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated x coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFxStandardDeviation() {
final var variance = getEstimatedBiasFxVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias.
*
* @return standard deviation of estimated x coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFxStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFxStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFxStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFxStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated y coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFyVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated y coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFyStandardDeviation() {
final var variance = getEstimatedBiasFyVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias.
*
* @return standard deviation of estimated y coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFyStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFyStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFyStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFyStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated z coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFzVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated z coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFzStandardDeviation() {
final var variance = getEstimatedBiasFzVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias.
*
* @return standard deviation of estimated z coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFzStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFzStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFzStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFzStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated accelerometer bias coordinates.
*
* @return standard deviation of estimated accelerometer bias coordinates.
*/
public AccelerationTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null ?
new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
getEstimatedBiasFxStandardDeviation(),
getEstimatedBiasFyStandardDeviation(),
getEstimatedBiasFzStandardDeviation()) : null;
}
/**
* Gets standard deviation of estimated accelerometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of accelerometer bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AccelerationTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasFxStandardDeviation(),
getEstimatedBiasFyStandardDeviation(),
getEstimatedBiasFzStandardDeviation(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates expressed
* in meters per squared second (m/s^2).
*
* @return average of estimated standard deviation of accelerometer bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null ?
(getEstimatedBiasFxStandardDeviation() + getEstimatedBiasFyStandardDeviation()
+ getEstimatedBiasFzStandardDeviation()) / 3.0 : null;
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates.
*
* @return average of estimated standard deviation of accelerometer bias coordinates or null.
*/
public Acceleration getEstimatedBiasStandardDeviationAverageAsAcceleration() {
return estimatedCovariance != null ?
new Acceleration(getEstimatedBiasStandardDeviationAverage(),
AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of accelerometer bias expressed in
* meters per squared second (m/s^2).
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of accelerometer bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasFxVariance() + getEstimatedBiasFyVariance() + getEstimatedBiasFzVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of accelerometer bias.
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of accelerometer bias or null
* if not available.
*/
public Acceleration getEstimatedBiasStandardDeviationNormAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasStandardDeviationNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of accelerometer bias coordinates.
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of accelerometer bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3225 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4157 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where estimated gyroscope biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return column matrix containing x,y,z components of estimated gyroscope
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasX() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasY() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasZ() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedX() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedY() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedZ() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope bias.
*
* @return estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeedTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if estimated gyroscope bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null
? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated x coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasXStandardDeviation() {
final var variance = getEstimatedBiasXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasXStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated y coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasYStandardDeviation() {
final var variance = getEstimatedBiasYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasYStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated z coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasZStandardDeviation() {
final var variance = getEstimatedBiasZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasZStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @return standard deviation of estimated gyroscope bias coordinates.
*/
public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of gyroscope bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation(),
AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates expressed
* in radians per second (rad/s).
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
+ getEstimatedBiasZStandardDeviation()) / 3.0
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null.
*/
public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of gyroscope bias expressed in
* radians per second (rad/s).
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias coordinates.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of gyroscope bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Internal method to perform calibration when common z-axis is assumed for both
* the accelerometer and gyroscope.
*
* @throws AlgebraException if there are numerical errors.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateCommonAxis() throws AlgebraException, FittingException, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2141 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3717 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @return array containing x,y,z components of estimated magnetometer
* hard-iron biases.
*/
@Override
public double[] getEstimatedHardIron() {
return estimatedHardIron;
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @param result instance where estimated magnetometer biases will be
* stored.
* @return true if result instance was updated, false otherwise (when
* estimation is not yet available).
*/
@Override
public boolean getEstimatedHardIron(final double[] result) {
if (estimatedHardIron != null) {
System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @return column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases.
*/
@Override
public Matrix getEstimatedHardIronAsMatrix() {
return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedHardIron != null) {
result.fromArray(estimatedHardIron);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return x coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronX() {
return estimatedHardIron != null ? estimatedHardIron[0] : null;
}
/**
* Gets y coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return y coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronY() {
return estimatedHardIron != null ? estimatedHardIron[1] : null;
}
/**
* Gets z coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return z coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronZ() {
return estimatedHardIron != null ? estimatedHardIron[2] : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @return x coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[0]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @return y coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[1]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @return z coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[2]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer bias.
*
* @return estimated magnetometer bias or null if not available.
*/
@Override
public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
return estimatedHardIron != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2])
: null;
}
/**
* Gets estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
if (estimatedHardIron != null) {
result.setValueCoordinatesAndUnit(estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2],
MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets variance of estimated x coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated x coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated x coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronXStandardDeviation() {
final var variance = getEstimatedHardIronXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias.
*
* @return standard deviation of estimated x coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronXStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity(
final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronXStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated y coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated y coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronYStandardDeviation() {
final var variance = getEstimatedHardIronYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias.
*
* @return standard deviation of estimated y coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronYStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity(
final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronYStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated z coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated z coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronZStandardDeviation() {
final var variance = getEstimatedHardIronZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias.
*
* @return standard deviation of estimated z coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronZStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronZStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated magnetometer bias coordinates.
*
* @return standard deviation of estimated magnetometer bias coordinates.
*/
public MagneticFluxDensityTriad getEstimatedHardIronStandardDeviation() {
return estimatedCovariance != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
getEstimatedHardIronXStandardDeviation(),
getEstimatedHardIronYStandardDeviation(),
getEstimatedHardIronZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated magnetometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of magnetometer bias was available,
* false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviation(final MagneticFluxDensityTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedHardIronXStandardDeviation(),
getEstimatedHardIronYStandardDeviation(),
getEstimatedHardIronZStandardDeviation(),
MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates
* expressed in Teslas (T).
*
* @return average of estimated standard deviation of magnetometer bias coordinates,
* or null if not available.
*/
public Double getEstimatedHardIronStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedHardIronXStandardDeviation() + getEstimatedHardIronYStandardDeviation()
+ getEstimatedHardIronZStandardDeviation()) / 3.0
: null;
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates.
*
* @return average of estimated standard deviation of magnetometer bias coordinates,
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationAverage(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of magnetometer bias is available,
* false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronStandardDeviationAverage());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of magnetometer bias expressed in
* Teslas (T).
*
* @return norm of estimated standard deviation of magnetometer bias or null
* if not available.
*/
public Double getEstimatedHardIronStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedHardIronXVariance() + getEstimatedHardIronYVariance()
+ getEstimatedHardIronZVariance()) : null;
}
/**
* Gets norm of estimated standard deviation of magnetometer bias.
*
* @return norm of estimated standard deviation of magnetometer bias or null
* if not available.
*/
public MagneticFluxDensity getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationNorm(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets norm of estimated standard deviation of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of magnetometer bias
* is available, false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity(
final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronStandardDeviationNorm());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java | 1354 |
| com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java | 1499 |
public void getEcefVelocity(final ECEFVelocity result) {
result.setCoordinates(vx, vy, vz);
}
/**
* Gets estimated ECEF user velocity.
*
* @return estimated ECEF user velocity.
*/
public ECEFVelocity getEcefVelocity() {
return new ECEFVelocity(vx, vy, vz);
}
/**
* Sets estimated ECEF user velocity.
*
* @param ecefVelocity estimated ECEF user velocity.
*/
public void setEcefVelocity(final ECEFVelocity ecefVelocity) {
vx = ecefVelocity.getVx();
vy = ecefVelocity.getVy();
vz = ecefVelocity.getVz();
}
/**
* Gets x coordinate of estimated ECEF user position.
*
* @param result instance where x coordinate of estimated ECEF user position
* will be stored.
*/
public void getDistanceX(final Distance result) {
result.setValue(x);
result.setUnit(DistanceUnit.METER);
}
/**
* Gets x coordinate of estimated ECEF user position.
*
* @return x coordinate of estimated ECEF user position.
*/
public Distance getDistanceX() {
return new Distance(x, DistanceUnit.METER);
}
/**
* Sets x coordinate of estimated ECEF user position.
*
* @param x x coordinate of estimated ECEF user position.
*/
public void setDistanceX(final Distance x) {
this.x = DistanceConverter.convert(x.getValue().doubleValue(), x.getUnit(), DistanceUnit.METER);
}
/**
* Gets y coordinate of estimated ECEF user position.
*
* @param result instance where y coordinate of estimated ECEF user position
* will be stored.
*/
public void getDistanceY(final Distance result) {
result.setValue(y);
result.setUnit(DistanceUnit.METER);
}
/**
* Gets y coordinate of estimated ECEF user position.
*
* @return y coordinate of estimated ECEF user position.
*/
public Distance getDistanceY() {
return new Distance(y, DistanceUnit.METER);
}
/**
* Sets y coordinate of estimated ECEF user position.
*
* @param y y coordinate of estimated ECEF user position.
*/
public void setDistanceY(final Distance y) {
this.y = DistanceConverter.convert(y.getValue().doubleValue(), y.getUnit(), DistanceUnit.METER);
}
/**
* Gets z coordinate of estimated ECEF user position.
*
* @param result instance where z coordinate of estimated ECEF user position
* will be stored.
*/
public void getDistanceZ(final Distance result) {
result.setValue(z);
result.setUnit(DistanceUnit.METER);
}
/**
* Gets z coordinate of estimated ECEF user position.
*
* @return z coordinate of estimated ECEF user position.
*/
public Distance getDistanceZ() {
return new Distance(z, DistanceUnit.METER);
}
/**
* Sets z coordinate of estimated ECEF user position.
*
* @param z z coordinate of estimated ECEF user position.
*/
public void setDistanceZ(final Distance z) {
this.z = DistanceConverter.convert(z.getValue().doubleValue(), z.getUnit(), DistanceUnit.METER);
}
/**
* Sets coordinates of estimated ECEF user position.
*
* @param x x coordinate of estimated ECEF user position.
* @param y y coordinate of estimated ECEF user position.
* @param z z coordinate of estimated ECEF user position.
*/
public void setPositionCoordinates(final Distance x, final Distance y, final Distance z) {
setDistanceX(x);
setDistanceY(y);
setDistanceZ(z);
}
/**
* Gets estimated ECEF user position expressed in meters (m).
*
* @param result instance where estimated ECEF user position expressed
* in meters (m) will be stored.
*/
public void getPosition(final Point3D result) {
result.setInhomogeneousCoordinates(x, y, z);
}
/**
* Gets estimated ECEF user position expressed in meters (m).
*
* @return estimated ECEF user position expressed in meters (m).
*/
public Point3D getPosition() {
return new InhomogeneousPoint3D(x, y, z);
}
/**
* Sets estimated ECEF user position expressed in meters (m).
*
* @param position estimated ECEF user position expressed in
* meters (m).
*/
public void setPosition(final Point3D position) {
x = position.getInhomX();
y = position.getInhomY();
z = position.getInhomZ();
}
/**
* Gets estimated ECEF user position.
*
* @param result instance where estimated ECEF user position
* will be stored.
*/
public void getEcefPosition(final ECEFPosition result) {
result.setCoordinates(x, y, z);
}
/**
* Gets estimated ECEF user position.
*
* @return estimated ECEF user position.
*/
public ECEFPosition getEcefPosition() {
return new ECEFPosition(x, y, z);
}
/**
* Sets estimated ECEF user position.
*
* @param ecefPosition estimated ECEF user position.
*/
public void setEcefPosition(final ECEFPosition ecefPosition) {
x = ecefPosition.getX();
y = ecefPosition.getY();
z = ecefPosition.getZ();
}
/**
* Gets estimated ECEF user position and velocity.
*
* @param result instance where estimated ECEF user position and velocity
* will be stored.
*/
public void getPositionAndVelocity(final ECEFPositionAndVelocity result) {
result.setPositionCoordinates(x, y, z);
result.setVelocityCoordinates(vx, vy, vz);
}
/**
* Gets estimated ECEF user position and velocity.
*
* @return estimated ECEF user position and velocity.
*/
public ECEFPositionAndVelocity getPositionAndVelocity() {
return new ECEFPositionAndVelocity(x, y, z, vx, vy, vz);
}
/**
* Sets estimated ECEF user position and velocity.
*
* @param positionAndVelocity estimated ECEF user position and velocity.
*/
public void setPositionAndVelocity(final ECEFPositionAndVelocity positionAndVelocity) {
x = positionAndVelocity.getX();
y = positionAndVelocity.getY();
z = positionAndVelocity.getZ();
vx = positionAndVelocity.getVx();
vy = positionAndVelocity.getVy();
vz = positionAndVelocity.getVz();
}
/**
* Gets body to ECEF frame containing coordinate transformation, position and
* velocity.
*
* @param result instance where body to ECEF frame will be stored.
* @return true if result was updated, false otherwise.
*/
public boolean getFrame(final ECEFFrame result) {
if (bodyToEcefCoordinateTransformationMatrix != null) {
try {
result.setCoordinateTransformation(getC());
} catch (final InvalidSourceAndDestinationFrameTypeException | InvalidRotationMatrixException e) {
return false;
}
result.setCoordinates(x, y, z);
result.setVelocityCoordinates(vx, vy, vz);
return true;
} else {
return false;
}
}
/**
* Gets body to ECEF frame containing coordinate transformation, position and
* velocity.
*
* @return body to ECEF frame.
*/
public ECEFFrame getFrame() {
if (bodyToEcefCoordinateTransformationMatrix != null) {
try {
return new ECEFFrame(x, y, z, vx, vy, vz, getC());
} catch (final InvalidSourceAndDestinationFrameTypeException | InvalidRotationMatrixException e) {
return null;
}
} else {
return null;
}
}
/**
* Sets body to ECEF frame containing coordinate transformation, position and
* velocity.
*
* @param frame body to ECEF frame to be set.
*/
public void setFrame(final ECEFFrame frame) {
x = frame.getX();
y = frame.getY();
z = frame.getZ();
vx = frame.getVx();
vy = frame.getVy();
vz = frame.getVz();
if (bodyToEcefCoordinateTransformationMatrix != null) {
frame.getCoordinateTransformationMatrix(bodyToEcefCoordinateTransformationMatrix);
} else {
bodyToEcefCoordinateTransformationMatrix = frame.getCoordinateTransformationMatrix();
}
}
/**
* Gets estimated accelerometer bias resolved around x axis.
*
* @param result instance where estimated accelerometer bias resolved around
* x axis will be stored.
*/
public void getAccelerationBiasXAsAcceleration(final Acceleration result) {
result.setValue(accelerationBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets estimated accelerometer bias resolved around x axis.
*
* @return estimated accelerometer bias resolved around x axis.
*/
public Acceleration getAccelerationBiasXAsAcceleration() {
return new Acceleration(accelerationBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets estimated accelerometer bias resolved around x axis.
*
* @param accelerationBiasX estimated accelerometer bias resolved
* around x axis.
*/
public void setAccelerationBiasX(final Acceleration accelerationBiasX) {
this.accelerationBiasX = AccelerationConverter.convert(accelerationBiasX.getValue().doubleValue(),
accelerationBiasX.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets estimated accelerometer bias resolved around y axis.
*
* @param result instance where estimated accelerometer bias resolved around
* y axis will be stored.
*/
public void getAccelerationBiasYAsAcceleration(final Acceleration result) {
result.setValue(accelerationBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets estimated accelerometer bias resolved around y axis.
*
* @return estimated accelerometer bias resolved around y axis.
*/
public Acceleration getAccelerationBiasYAsAcceleration() {
return new Acceleration(accelerationBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets estimated accelerometer bias resolved around y axis.
*
* @param accelerationBiasY estimated accelerometer bias resolved
* around y axis.
*/
public void setAccelerationBiasY(final Acceleration accelerationBiasY) {
this.accelerationBiasY = AccelerationConverter.convert(accelerationBiasY.getValue().doubleValue(),
accelerationBiasY.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets estimated accelerometer bias resolved around z axis.
*
* @param result instance where estimated accelerometer bias resolved around
* z axis will be stored.
*/
public void getAccelerationBiasZAsAcceleration(final Acceleration result) {
result.setValue(accelerationBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets estimated accelerometer bias resolved around z axis.
*
* @return estimated accelerometer bias resolved around z axis.
*/
public Acceleration getAccelerationBiasZAsAcceleration() {
return new Acceleration(accelerationBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets estimated accelerometer bias resolved around z axis.
*
* @param accelerationBiasZ estimated accelerometer bias resolved
* around z axis.
*/
public void setAccelerationBiasZ(final Acceleration accelerationBiasZ) {
this.accelerationBiasZ = AccelerationConverter.convert(accelerationBiasZ.getValue().doubleValue(),
accelerationBiasZ.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets estimated accelerometer bias coordinates.
*
* @param accelerationBiasX estimated accelerometer bias resolved around x axis.
* @param accelerationBiasY estimated accelerometer bias resolved around y axis.
* @param accelerationBiasZ estimated accelerometer bias resolved around z axis.
*/
public void setAccelerationBiasCoordinates(
final Acceleration accelerationBiasX, final Acceleration accelerationBiasY,
final Acceleration accelerationBiasZ) {
setAccelerationBiasX(accelerationBiasX);
setAccelerationBiasY(accelerationBiasY);
setAccelerationBiasZ(accelerationBiasZ);
}
/**
* Gets estimated gyroscope bias resolved around x axis.
*
* @param result instance where estimated gyroscope bias resolved around x axis will
* be stored.
*/
public void getAngularSpeedGyroBiasX(final AngularSpeed result) {
result.setValue(gyroBiasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets estimated gyroscope bias resolved around x axis.
*
* @return estimated gyroscope bias resolved around x axis.
*/
public AngularSpeed getAngularSpeedGyroBiasX() {
return new AngularSpeed(gyroBiasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets estimated gyroscope bias resolved around x axis.
*
* @param gyroBiasX estimated gyroscope bias resolved around x axis.
*/
public void setGyroBiasX(final AngularSpeed gyroBiasX) {
this.gyroBiasX = AngularSpeedConverter.convert(gyroBiasX.getValue().doubleValue(), gyroBiasX.getUnit(),
AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets estimated gyroscope bias resolved around y axis.
*
* @param result instance where estimated gyroscope bias resolved around y axis will
* be stored.
*/
public void getAngularSpeedGyroBiasY(final AngularSpeed result) {
result.setValue(gyroBiasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets estimated gyroscope bias resolved around y axis.
*
* @return estimated gyroscope bias resolved around y axis.
*/
public AngularSpeed getAngularSpeedGyroBiasY() {
return new AngularSpeed(gyroBiasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets estimated gyroscope bias resolved around y axis.
*
* @param gyroBiasY estimated gyroscope bias resolved around y axis.
*/
public void setGyroBiasY(final AngularSpeed gyroBiasY) {
this.gyroBiasY = AngularSpeedConverter.convert(gyroBiasY.getValue().doubleValue(), gyroBiasY.getUnit(),
AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets estimated gyroscope bias resolved around z axis.
*
* @param result instance where estimated gyroscope bias resolved around z axis will
* be stored.
*/
public void getAngularSpeedGyroBiasZ(final AngularSpeed result) {
result.setValue(gyroBiasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets estimated gyroscope bias resolved around z axis.
*
* @return estimated gyroscope bias resolved around z axis.
*/
public AngularSpeed getAngularSpeedGyroBiasZ() {
return new AngularSpeed(gyroBiasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets estimated gyroscope bias resolved around z axis.
*
* @param gyroBiasZ estimated gyroscope bias resolved around z axis.
*/
public void setGyroBiasZ(final AngularSpeed gyroBiasZ) {
this.gyroBiasZ = AngularSpeedConverter.convert(gyroBiasZ.getValue().doubleValue(), gyroBiasZ.getUnit(),
AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets estimated gyroscope bias coordinates.
*
* @param gyroBiasX estimated gyroscope bias resolved around x axis.
* @param gyroBiasY estimated gyroscope bias resolved around y axis.
* @param gyroBiasZ estimated gyroscope bias resolved around z axis.
*/
public void setGyroBiasCoordinates(
final AngularSpeed gyroBiasX, final AngularSpeed gyroBiasY, final AngularSpeed gyroBiasZ) {
setGyroBiasX(gyroBiasX);
setGyroBiasY(gyroBiasY);
setGyroBiasZ(gyroBiasZ);
}
/**
* Copies this instance data into provided instance.
*
* @param output destination instance where data will be copied to.
*/
public void copyTo(final INSLooselyCoupledKalmanState output) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4344 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2300 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 741 |
initialBiasX = convertAcceleration(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAcceleration(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* Values are expressed in meters per squared second (m/s^2).
*
* @return initial bias to be used to find a solution as a column matrix.
*/
@Override
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* Values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial bias to be used to find a solution as a column matrix with
* values expressed in meters per squared second (m/s^2).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4344 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2300 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1409 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1365 |
initialBiasX = convertAcceleration(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAcceleration(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* Values are expressed in meters per squared second (m/s^2).
*
* @return initial bias to be used to find a solution as a column matrix.
*/
@Override
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* Values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial bias to be used to find a solution as a column matrix with
* values expressed in meters per squared second (m/s^2).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3433 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3471 |
final double[] result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @return initial gyroscope bias to be used to find a solution as a
* column matrix.
*/
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* a column matrix with values expressed in radians per second (rad/s).
*
* @param initialBias initial gyroscope bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @return initial bias coordinates.
*/
public AngularSpeedTriad getInitialBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @param result instance where result will be stored.
*/
public void getInitialBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution.
*
* @param initialBias initial bias coordinates to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(final AngularSpeedTriad initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @return constant rotation rate of turntable.
*/
public double getTurntableRotationRate() {
return turntableRotationRate;
}
/**
* Sets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(final double turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
if (turntableRotationRate <= 0.0) {
throw new IllegalArgumentException();
}
this.turntableRotationRate = turntableRotationRate;
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @return constant rotation rate of turntable.
*/
public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @param result instance where result will be stored.
*/
public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
result.setValue(turntableRotationRate);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets constant rotation rate at which the turntable is spinning.
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
}
/**
* Gets time interval between measurements being captured expressed in
* seconds (s).
*
* @return time interval between measurements.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between measurements being captured expressed in
* seconds (s).
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval <= 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between measurements being captured.
*
* @return time interval between measurements.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between measurements being captured.
*
* @param result instance where result will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between measurements being captured.
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
setTimeInterval(convertTime(timeInterval));
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public List<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 711 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2651 |
public void setListener(final KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return x coordinate of gyroscope bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasX x coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return y coordinate of gyroscope bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasY y coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return z coordinate of gyroscope bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x coordinate of gyroscope bias.
*
* @return x coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedX() {
return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known x coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(biasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known x coordinate of gyroscope bias.
*
* @param biasX x coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final AngularSpeed biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
}
/**
* Gets known y coordinate of gyroscope bias.
*
* @return y coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedY() {
return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known y coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(biasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known y coordinate of gyroscope bias.
*
* @param biasY y coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final AngularSpeed biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAngularSpeed(biasY);
}
/**
* Gets known z coordinate of gyroscope bias.
*
* @return z coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedZ() {
return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known z coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(biasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known z coordinate of gyroscope bias.
*
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Sets known gyroscope bias coordinates expressed in radians per second
* (rad/s).
*
* @param biasX x coordinate of gyroscope bias.
* @param biasY y coordinate of gyroscope bias.
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known gyroscope bias coordinates.
*
* @param biasX x coordinate of gyroscope bias.
* @param biasY y coordinate of gyroscope bias.
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
this.biasY = convertAngularSpeed(biasY);
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Gets known gyroscope bias.
*
* @return known gyroscope bias.
*/
public AngularSpeedTriad getBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known gyroscope bias.
*
* @param result instance where result will be stored.
*/
public void getBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known gyroscope bias.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setBias(final AngularSpeedTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinate of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param bias known gyroscope bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known gyroscope bias as a column matrix.
*
* @return known gyroscope bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known gyroscope bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known gyroscope bias as a column matrix.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurementsOrSequences() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the calibration.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/AccelerationFixer.java | 382 |
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 398 |
public void setBias(final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
setBiasX(biasX);
setBiasY(biasY);
setBiasZ(biasZ);
}
/**
* Gets cross coupling errors matrix.
*
* @return cross coupling errors matrix.
*/
public Matrix getCrossCouplingErrors() {
return new Matrix(crossCouplingErrors);
}
/**
* Gets cross coupling errors matrix.
*
* @param result instance where result will be stored.
*/
public void getCrossCouplingErrors(final Matrix result) {
crossCouplingErrors.copyTo(result);
}
/**
* Sets cross coupling errors matrix.
*
* @param crossCouplingErrors cross coupling errors matrix. Must be 3x3.
* @throws AlgebraException if provided matrix cannot be inverted.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
public void setCrossCouplingErrors(final Matrix crossCouplingErrors) throws AlgebraException {
if (crossCouplingErrors.getRows() != BodyKinematics.COMPONENTS
|| crossCouplingErrors.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
this.crossCouplingErrors = crossCouplingErrors;
identity.add(crossCouplingErrors, tmp1);
Utils.inverse(tmp1, tmp2);
}
/**
* Gets x scaling factor.
*
* @return x scaling factor.
*/
public double getSx() {
return crossCouplingErrors.getElementAt(0, 0);
}
/**
* Sets x scaling factor
*
* @param sx x scaling factor.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setSx(final double sx) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 0, sx);
setCrossCouplingErrors(m);
}
/**
* Gets y scaling factor.
*
* @return y scaling factor.
*/
public double getSy() {
return crossCouplingErrors.getElementAt(1, 1);
}
/**
* Sets y scaling factor.
*
* @param sy y scaling factor.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setSy(final double sy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(1, 1, sy);
setCrossCouplingErrors(m);
}
/**
* Gets z scaling factor.
*
* @return z scaling factor.
*/
public double getSz() {
return crossCouplingErrors.getElementAt(2, 2);
}
/**
* Sets z scaling factor.
*
* @param sz z scaling factor.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setSz(final double sz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(2, 2, sz);
setCrossCouplingErrors(m);
}
/**
* Gets x-y cross coupling error.
*
* @return x-y cross coupling error.
*/
public double getMxy() {
return crossCouplingErrors.getElementAt(0, 1);
}
/**
* Sets x-y cross coupling error.
*
* @param mxy x-y cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMxy(final double mxy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 1, mxy);
setCrossCouplingErrors(m);
}
/**
* Gets x-z cross coupling error.
*
* @return x-z cross coupling error.
*/
public double getMxz() {
return crossCouplingErrors.getElementAt(0, 2);
}
/**
* Sets x-z cross coupling error.
*
* @param mxz x-z cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMxz(final double mxz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 2, mxz);
setCrossCouplingErrors(m);
}
/**
* Gets y-x cross coupling error.
*
* @return y-x cross coupling error.
*/
public double getMyx() {
return crossCouplingErrors.getElementAt(1, 0);
}
/**
* Sets y-x cross coupling error.
*
* @param myx y-x cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMyx(final double myx) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(1, 0, myx);
setCrossCouplingErrors(m);
}
/**
* Gets y-z cross coupling error.
*
* @return y-z cross coupling error.
*/
public double getMyz() {
return crossCouplingErrors.getElementAt(1, 2);
}
/**
* Sets y-z cross coupling error.
*
* @param myz y-z cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMyz(final double myz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(1, 2, myz);
setCrossCouplingErrors(m);
}
/**
* Gets z-x cross coupling error.
*
* @return z-x cross coupling error.
*/
public double getMzx() {
return crossCouplingErrors.getElementAt(2, 0);
}
/**
* Sets z-x cross coupling error.
*
* @param mzx z-x cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMzx(final double mzx) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(2, 0, mzx);
setCrossCouplingErrors(m);
}
/**
* Gets z-y cross coupling error.
*
* @return z-y cross coupling error.
*/
public double getMzy() {
return crossCouplingErrors.getElementAt(2, 1);
}
/**
* Sets z-y cross coupling error.
*
* @param mzy z-y cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMzy(final double mzy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(2, 1, mzy);
setCrossCouplingErrors(m);
}
/**
* Sets scaling factors.
*
* @param sx x scaling factor.
* @param sy y scaling factor.
* @param sz z scaling factor.
* @throws AlgebraException if provided values make cross coupling matrix
* non invertible.
*/
public void setScalingFactors(final double sx, final double sy, final double sz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 0, sx);
m.setElementAt(1, 1, sy);
m.setElementAt(2, 2, sz);
setCrossCouplingErrors(m);
}
/**
* Sets cross coupling errors.
*
* @param mxy x-y cross coupling error.
* @param mxz x-z cross coupling error.
* @param myx y-x cross coupling error.
* @param myz y-z cross coupling error.
* @param mzx z-x cross coupling error.
* @param mzy z-y cross coupling error.
* @throws AlgebraException if provided values make cross coupling matrix
* non invertible.
*/
public void setCrossCouplingErrors(
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 1, mxy);
m.setElementAt(0, 2, mxz);
m.setElementAt(1, 0, myx);
m.setElementAt(1, 2, myz);
m.setElementAt(2, 0, mzx);
m.setElementAt(2, 1, mzy);
setCrossCouplingErrors(m);
}
/**
* Sets scaling factors and cross coupling errors.
*
* @param sx x scaling factor.
* @param sy y scaling factor.
* @param sz z scaling factor.
* @param mxy x-y cross coupling error.
* @param mxz x-z cross coupling error.
* @param myx y-x cross coupling error.
* @param myz y-z cross coupling error.
* @param mzx z-x cross coupling error.
* @param mzy z-y cross coupling error.
* @throws AlgebraException if provided values make cross coupling matrix
* non invertible.
*/
public void setScalingFactorsAndCrossCouplingErrors(
final double sx, final double sy, final double sz,
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 0, sx);
m.setElementAt(1, 1, sy);
m.setElementAt(2, 2, sz);
m.setElementAt(0, 1, mxy);
m.setElementAt(0, 2, mxz);
m.setElementAt(1, 0, myx);
m.setElementAt(1, 2, myz);
m.setElementAt(2, 0, mzx);
m.setElementAt(2, 1, mzy);
setCrossCouplingErrors(m);
}
/**
* Fixes provided measured specific force values by undoing the errors
* introduced by the accelerometer model to restore the true specific
* force.
* This method uses last provided bias and cross coupling errors.
*
* @param measuredF measured specific force.
* @param result instance where restored true specific force will be stored.
* Mut have length 3.
* @throws AlgebraException if there are numerical instabilities.
* @throws IllegalArgumentException if length of provided result array is
* not 3.
*/
public void fix(final AccelerationTriad measuredF, final double[] result) throws AlgebraException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3403 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4730 |
setInputData();
fitter.fit();
final var result = fitter.getA();
final var m11 = result[0];
final var m21 = result[1];
final var m31 = result[2];
final var m12 = result[3];
final var m22 = result[4];
final var m32 = result[5];
final var m13 = result[6];
final var m23 = result[7];
final var m33 = result[8];
final var g11 = result[9];
final var g21 = result[10];
final var g31 = result[11];
final var g12 = result[12];
final var g22 = result[13];
final var g32 = result[14];
final var g13 = result[15];
final var g23 = result[16];
final var g33 = result[17];
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mm.setElementAtIndex(0, m11);
mm.setElementAtIndex(1, m21);
mm.setElementAtIndex(2, m31);
mm.setElementAtIndex(3, m12);
mm.setElementAtIndex(4, m22);
mm.setElementAtIndex(5, m32);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33);
final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mg.setElementAtIndex(0, g11);
mg.setElementAtIndex(1, g21);
mg.setElementAtIndex(2, g31);
mg.setElementAtIndex(3, g12);
mg.setElementAtIndex(4, g22);
mg.setElementAtIndex(5, g32);
mg.setElementAtIndex(6, g13);
mg.setElementAtIndex(7, g23);
mg.setElementAtIndex(8, g33);
setResult(mm, mg);
// at this point covariance is expressed in terms of M and G, and must
// be expressed in terms of Mg and Gg.
// We know that:
// Mg = M - I
// Gg = M * G
// M = [m11 m12 m13]
// [m21 m22 m23]
// [m31 m32 m33]
// G = [g11 g12 g13]
// [g21 g22 g23]
// [g31 g32 g33]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [m21 m22 - 1 m23 ]
// [mzx mzy sz ] [m31 m32 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = [m11 m12 m13][g11 g12 g13]
// [gg21 gg22 gg23] [m21 m22 m23][g21 g22 g23]
// [gg31 gg32 gg33] [m31 m32 m33][g31 g32 g33]
// Defining the linear application:
// F(M, G) = F(m11, m21, m31, m12, m22, m32, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
// as:
// [sx] = [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [m21]
// [myz] [m23]
// [mzx] [m31]
// [mzy] [m32]
// [gg11] [m11 * g11 + m12 * g21 + m13 * g31]
// [gg21] [m21 * g11 + m22 * g21 + m23 * g31]
// [gg31] [m31 * g11 + m32 * g21 + m33 * g31]
// [gg12] [m11 * g12 + m12 * g22 + m13 * g32]
// [gg22] [m21 * g12 + m22 * g22 + n23 * g32]
// [gg32] [m31 * g12 + m32 * g22 + m33 * g32]
// [gg13] [m11 * g13 + m12 * g23 + m13 * g33]
// [gg23] [m21 * g13 + m22 * g23 + m23 * g33]
// [gg33] [m31 * g13 + m32 * g23 + m33 * g33]
// Then the Jacobian of F(M, G) is:
// J = [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [g11 0 0 g21 0 0 g31 0 0 m11 m12 m13 0 0 0 0 0 0 ]
// [0 g11 0 0 g21 0 0 g31 0 m21 m22 m23 0 0 0 0 0 0 ]
// [0 0 g11 0 0 g21 0 0 g31 m31 m32 m33 0 0 0 0 0 0 ]
// [g12 0 0 g22 0 0 g32 0 0 0 0 0 m11 m12 m13 0 0 0 ]
// [0 g12 0 0 g22 0 0 g32 0 0 0 0 m21 m22 m23 0 0 0 ]
// [0 0 g12 0 0 g22 0 0 g32 0 0 0 m31 m32 m33 0 0 0 ]
// [g13 0 0 g23 0 0 g33 0 0 0 0 0 0 0 0 m11 m12 m13]
// [0 g13 0 0 g23 0 0 g33 0 0 0 0 0 0 0 m21 m22 m23]
// [0 0 g13 0 0 g23 0 0 g33 0 0 0 0 0 0 m31 m32 m33]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, GENERAL_UNKNOWNS_AND_CROSS_BIASES);
jacobian.setElementAt(0, 0, 1.0);
jacobian.setElementAt(1, 4, 1.0);
jacobian.setElementAt(2, 8, 1.0);
jacobian.setElementAt(3, 3, 1.0);
jacobian.setElementAt(4, 6, 1.0);
jacobian.setElementAt(5, 1, 1.0);
jacobian.setElementAt(6, 7, 1.0);
jacobian.setElementAt(7, 2, 1.0);
jacobian.setElementAt(8, 5, 1.9);
jacobian.setElementAt(9, 0, g11);
jacobian.setElementAt(9, 3, g21);
jacobian.setElementAt(9, 6, g31);
jacobian.setElementAt(9, 9, m11);
jacobian.setElementAt(9, 10, m12);
jacobian.setElementAt(9, 11, m13);
jacobian.setElementAt(10, 1, g11);
jacobian.setElementAt(10, 4, g21);
jacobian.setElementAt(10, 7, g31);
jacobian.setElementAt(10, 9, m21);
jacobian.setElementAt(10, 10, m22);
jacobian.setElementAt(10, 11, m23);
jacobian.setElementAt(11, 2, g11);
jacobian.setElementAt(11, 5, g21);
jacobian.setElementAt(11, 8, g31);
jacobian.setElementAt(11, 9, m31);
jacobian.setElementAt(11, 10, m32);
jacobian.setElementAt(11, 11, m33);
jacobian.setElementAt(12, 0, g12);
jacobian.setElementAt(12, 3, g22);
jacobian.setElementAt(12, 6, g32);
jacobian.setElementAt(12, 12, m11);
jacobian.setElementAt(12, 13, m12);
jacobian.setElementAt(12, 14, m13);
jacobian.setElementAt(13, 1, g12);
jacobian.setElementAt(13, 4, g22);
jacobian.setElementAt(13, 7, g32);
jacobian.setElementAt(13, 12, m21);
jacobian.setElementAt(13, 13, m22);
jacobian.setElementAt(13, 14, m23);
jacobian.setElementAt(14, 2, g12);
jacobian.setElementAt(14, 5, g22);
jacobian.setElementAt(14, 8, g32);
jacobian.setElementAt(14, 12, m31);
jacobian.setElementAt(14, 13, m32);
jacobian.setElementAt(14, 14, m33);
jacobian.setElementAt(15, 0, g13);
jacobian.setElementAt(15, 3, g23);
jacobian.setElementAt(15, 6, g33);
jacobian.setElementAt(15, 15, m11);
jacobian.setElementAt(15, 16, m12);
jacobian.setElementAt(15, 17, m13);
jacobian.setElementAt(16, 1, g13);
jacobian.setElementAt(16, 4, g23);
jacobian.setElementAt(16, 7, g33);
jacobian.setElementAt(16, 15, m21);
jacobian.setElementAt(16, 16, m22);
jacobian.setElementAt(16, 17, m23);
jacobian.setElementAt(17, 2, g13);
jacobian.setElementAt(17, 5, g23);
jacobian.setElementAt(17, 8, g33);
jacobian.setElementAt(17, 15, m31);
jacobian.setElementAt(17, 16, m32);
jacobian.setElementAt(17, 17, m33);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Internal method to perform calibration when common z-axis is assumed
* for both the accelerometer and gyroscope and G-dependent cross biases
* are ignored.
*
* @throws AlgebraException if accelerometer parameters prevent fixing
* measured accelerometer values due to numerical instabilities.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateCommonAxis() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2793 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4054 |
return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where estimated gyroscope biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return column matrix containing x,y,z components of estimated gyroscope
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasX() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasY() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasZ() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedX() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedY() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedZ() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope bias.
*
* @return estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeedTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if estimated gyroscope bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2804 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1514 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where estimated gyroscope biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return column matrix containing x,y,z components of estimated gyroscope
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasX() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasY() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasZ() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedX() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedY() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedZ() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope bias.
*
* @return estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeedTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if estimated gyroscope bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2114 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1383 |
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz,
final double initialMyx, final double initialMyz,
final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<StandardDeviationFrameBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3556 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3603 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @return constant rotation rate of turntable.
*/
public double getTurntableRotationRate() {
return turntableRotationRate;
}
/**
* Sets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(final double turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
if (turntableRotationRate <= 0.0) {
throw new IllegalArgumentException();
}
this.turntableRotationRate = turntableRotationRate;
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @return constant rotation rate of turntable.
*/
public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @param result instance where result will be stored.
*/
public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
result.setValue(turntableRotationRate);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets constant rotation rate at which the turntable is spinning.
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
}
/**
* Gets time interval between measurements being captured expressed in
* seconds (s).
*
* @return time interval between measurements.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between measurements being captured expressed in
* seconds (s).
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval <= 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between measurements being captured.
*
* @return time interval between measurements.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between measurements being captured.
*
* @param result instance where result will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between measurements being captured.
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
setTimeInterval(convertTime(timeInterval));
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @param measurements collection of body kinematics measurements at a
* known position with unknown orientations.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final Collection<StandardDeviationBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @return position where body kinematics measures have been taken.
*/
public ECEFPosition getEcefPosition() {
return position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final ECEFPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @return position where body kinematics measures have been taken or null if
* not available.
*/
public NEDPosition getNedPosition() {
final var result = new NEDPosition();
return getNedPosition(result) ? result : null;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param result instance where result will be stored.
* @return true if NED position could be computed, false otherwise.
*/
public boolean getNedPosition(final NEDPosition result) {
if (position != null) {
final var velocity = new NEDVelocity();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
position.getX(), position.getY(), position.getZ(), 0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final NEDPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = convertPosition(position);
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_BODY_KINEMATICS_MEASUREMENT;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Indicates whether G-dependent cross biases are being estimated
* or not.
* When enabled, this adds 9 variables from Gg matrix.
*
* @return true if G-dependent cross biases will be estimated,
* false otherwise.
*/
public boolean isGDependentCrossBiasesEstimated() {
return estimateGDependentCrossBiases;
}
/**
* Specifies whether G-dependent cross biases are being estimated
* or not.
* When enabled, this adds 9 variables from Gg matrix.
*
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setGDependentCrossBiasesEstimated(final boolean estimateGDependentCrossBiases) throws LockedException {
if (running) {
throw new LockedException();
}
this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public KnownBiasTurntableGyroscopeCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3515 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3567 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @return constant rotation rate of turntable.
*/
public double getTurntableRotationRate() {
return turntableRotationRate;
}
/**
* Sets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(
final double turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
if (turntableRotationRate <= 0.0) {
throw new IllegalArgumentException();
}
this.turntableRotationRate = turntableRotationRate;
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @return constant rotation rate of turntable.
*/
public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @param result instance where result will be stored.
*/
public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
result.setValue(turntableRotationRate);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets constant rotation rate at which the turntable is spinning.
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
}
/**
* Gets time interval between measurements being captured expressed in
* seconds (s).
*
* @return time interval between measurements.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between measurements being captured expressed in
* seconds (s).
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval <= 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between measurements being captured.
*
* @return time interval between measurements.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between measurements being captured.
*
* @param result instance where result will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between measurements being captured.
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
setTimeInterval(convertTime(timeInterval));
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public List<StandardDeviationBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @param measurements collection of body kinematics measurements at a
* known position with unknown orientations.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final List<StandardDeviationBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @return position where body kinematics measures have been taken.
*/
public ECEFPosition getEcefPosition() {
return position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final ECEFPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @return position where body kinematics measures have been taken or null if
* not available.
*/
public NEDPosition getNedPosition() {
final var result = new NEDPosition();
return getNedPosition(result) ? result : null;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param result instance where result will be stored.
* @return true if NED position could be computed, false otherwise.
*/
public boolean getNedPosition(final NEDPosition result) {
if (position != null) {
final var velocity = new NEDVelocity();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
position.getX(), position.getY(), position.getZ(), 0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final NEDPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = convertPosition(position);
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_BODY_KINEMATICS_MEASUREMENT;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Indicates whether G-dependent cross biases are being estimated
* or not.
* When enabled, this adds 9 variables from Gg matrix.
*
* @return true if G-dependent cross biases will be estimated,
* false otherwise.
*/
public boolean isGDependentCrossBiasesEstimated() {
return estimateGDependentCrossBiases;
}
/**
* Specifies whether G-dependent cross biases are being estimated
* or not.
* When enabled, this adds 9 variables from Gg matrix.
*
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setGDependentCrossBiasesEstimated(final boolean estimateGDependentCrossBiases) throws LockedException {
if (running) {
throw new LockedException();
}
this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public RobustKnownBiasTurntableGyroscopeCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/AccelerationFixer.java | 415 |
| com/irurueta/navigation/inertial/calibration/MagneticFluxDensityFixer.java | 431 |
|| crossCouplingErrors.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
this.crossCouplingErrors = crossCouplingErrors;
identity.add(crossCouplingErrors, tmp1);
Utils.inverse(tmp1, tmp2);
}
/**
* Gets x scaling factor.
*
* @return x scaling factor.
*/
public double getSx() {
return crossCouplingErrors.getElementAt(0, 0);
}
/**
* Sets x scaling factor
*
* @param sx x scaling factor.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setSx(final double sx) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 0, sx);
setCrossCouplingErrors(m);
}
/**
* Gets y scaling factor.
*
* @return y scaling factor.
*/
public double getSy() {
return crossCouplingErrors.getElementAt(1, 1);
}
/**
* Sets y scaling factor.
*
* @param sy y scaling factor.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setSy(final double sy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(1, 1, sy);
setCrossCouplingErrors(m);
}
/**
* Gets z scaling factor.
*
* @return z scaling factor.
*/
public double getSz() {
return crossCouplingErrors.getElementAt(2, 2);
}
/**
* Sets z scaling factor.
*
* @param sz z scaling factor.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setSz(final double sz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(2, 2, sz);
setCrossCouplingErrors(m);
}
/**
* Gets x-y cross coupling error.
*
* @return x-y cross coupling error.
*/
public double getMxy() {
return crossCouplingErrors.getElementAt(0, 1);
}
/**
* Sets x-y cross coupling error.
*
* @param mxy x-y cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMxy(final double mxy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 1, mxy);
setCrossCouplingErrors(m);
}
/**
* Gets x-z cross coupling error.
*
* @return x-z cross coupling error.
*/
public double getMxz() {
return crossCouplingErrors.getElementAt(0, 2);
}
/**
* Sets x-z cross coupling error.
*
* @param mxz x-z cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMxz(final double mxz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 2, mxz);
setCrossCouplingErrors(m);
}
/**
* Gets y-x cross coupling error.
*
* @return y-x cross coupling error.
*/
public double getMyx() {
return crossCouplingErrors.getElementAt(1, 0);
}
/**
* Sets y-x cross coupling error.
*
* @param myx y-x cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMyx(final double myx) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(1, 0, myx);
setCrossCouplingErrors(m);
}
/**
* Gets y-z cross coupling error.
*
* @return y-z cross coupling error.
*/
public double getMyz() {
return crossCouplingErrors.getElementAt(1, 2);
}
/**
* Sets y-z cross coupling error.
*
* @param myz y-z cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMyz(final double myz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(1, 2, myz);
setCrossCouplingErrors(m);
}
/**
* Gets z-x cross coupling error.
*
* @return z-x cross coupling error.
*/
public double getMzx() {
return crossCouplingErrors.getElementAt(2, 0);
}
/**
* Sets z-x cross coupling error.
*
* @param mzx z-x cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMzx(final double mzx) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(2, 0, mzx);
setCrossCouplingErrors(m);
}
/**
* Gets z-y cross coupling error.
*
* @return z-y cross coupling error.
*/
public double getMzy() {
return crossCouplingErrors.getElementAt(2, 1);
}
/**
* Sets z-y cross coupling error.
*
* @param mzy z-y cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non invertible.
*/
public void setMzy(final double mzy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(2, 1, mzy);
setCrossCouplingErrors(m);
}
/**
* Sets scaling factors.
*
* @param sx x scaling factor.
* @param sy y scaling factor.
* @param sz z scaling factor.
* @throws AlgebraException if provided values make cross coupling matrix
* non invertible.
*/
public void setScalingFactors(final double sx, final double sy, final double sz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 0, sx);
m.setElementAt(1, 1, sy);
m.setElementAt(2, 2, sz);
setCrossCouplingErrors(m);
}
/**
* Sets cross coupling errors.
*
* @param mxy x-y cross coupling error.
* @param mxz x-z cross coupling error.
* @param myx y-x cross coupling error.
* @param myz y-z cross coupling error.
* @param mzx z-x cross coupling error.
* @param mzy z-y cross coupling error.
* @throws AlgebraException if provided values make cross coupling matrix
* non invertible.
*/
public void setCrossCouplingErrors(
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 1, mxy);
m.setElementAt(0, 2, mxz);
m.setElementAt(1, 0, myx);
m.setElementAt(1, 2, myz);
m.setElementAt(2, 0, mzx);
m.setElementAt(2, 1, mzy);
setCrossCouplingErrors(m);
}
/**
* Sets scaling factors and cross coupling errors.
*
* @param sx x scaling factor.
* @param sy y scaling factor.
* @param sz z scaling factor.
* @param mxy x-y cross coupling error.
* @param mxz x-z cross coupling error.
* @param myx y-x cross coupling error.
* @param myz y-z cross coupling error.
* @param mzx z-x cross coupling error.
* @param mzy z-y cross coupling error.
* @throws AlgebraException if provided values make cross coupling matrix
* non invertible.
*/
public void setScalingFactorsAndCrossCouplingErrors(
final double sx, final double sy, final double sz,
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 0, sx);
m.setElementAt(1, 1, sy);
m.setElementAt(2, 2, sz);
m.setElementAt(0, 1, mxy);
m.setElementAt(0, 2, mxz);
m.setElementAt(1, 0, myx);
m.setElementAt(1, 2, myz);
m.setElementAt(2, 0, mzx);
m.setElementAt(2, 1, mzy);
setCrossCouplingErrors(m);
}
/**
* Fixes provided measured specific force values by undoing the errors
* introduced by the accelerometer model to restore the true specific
* force.
* This method uses last provided bias and cross coupling errors.
*
* @param measuredF measured specific force.
* @param result instance where restored true specific force will be stored.
* Mut have length 3.
* @throws AlgebraException if there are numerical instabilities.
* @throws IllegalArgumentException if length of provided result array is
* not 3.
*/
public void fix(final AccelerationTriad measuredF, final double[] result) throws AlgebraException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 431 |
| com/irurueta/navigation/inertial/calibration/MagneticFluxDensityFixer.java | 431 |
|| crossCouplingErrors.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
this.crossCouplingErrors = crossCouplingErrors;
identity.add(crossCouplingErrors, tmp1);
Utils.inverse(tmp1, tmp2);
}
/**
* Gets x scaling factor.
*
* @return x scaling factor.
*/
public double getSx() {
return crossCouplingErrors.getElementAt(0, 0);
}
/**
* Sets x scaling factor
*
* @param sx x scaling factor.
* @throws AlgebraException if provided value makes cross coupling matrix
* non-invertible.
*/
public void setSx(final double sx) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 0, sx);
setCrossCouplingErrors(m);
}
/**
* Gets y scaling factor.
*
* @return y scaling factor.
*/
public double getSy() {
return crossCouplingErrors.getElementAt(1, 1);
}
/**
* Sets y scaling factor.
*
* @param sy y scaling factor.
* @throws AlgebraException if provided value makes cross coupling matrix
* non-invertible.
*/
public void setSy(final double sy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(1, 1, sy);
setCrossCouplingErrors(m);
}
/**
* Gets z scaling factor.
*
* @return z scaling factor.
*/
public double getSz() {
return crossCouplingErrors.getElementAt(2, 2);
}
/**
* Sets z scaling factor.
*
* @param sz z scaling factor.
* @throws AlgebraException if provided value makes cross coupling matrix
* non-invertible.
*/
public void setSz(final double sz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(2, 2, sz);
setCrossCouplingErrors(m);
}
/**
* Gets x-y cross coupling error.
*
* @return x-y cross coupling error.
*/
public double getMxy() {
return crossCouplingErrors.getElementAt(0, 1);
}
/**
* Sets x-y cross coupling error.
*
* @param mxy x-y cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non-invertible.
*/
public void setMxy(final double mxy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 1, mxy);
setCrossCouplingErrors(m);
}
/**
* Gets x-z cross coupling error.
*
* @return x-z cross coupling error.
*/
public double getMxz() {
return crossCouplingErrors.getElementAt(0, 2);
}
/**
* Sets x-z cross coupling error.
*
* @param mxz x-z cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non-invertible.
*/
public void setMxz(final double mxz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 2, mxz);
setCrossCouplingErrors(m);
}
/**
* Gets y-x cross coupling error.
*
* @return y-x cross coupling error.
*/
public double getMyx() {
return crossCouplingErrors.getElementAt(1, 0);
}
/**
* Sets y-x cross coupling error.
*
* @param myx y-x cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non-invertible.
*/
public void setMyx(final double myx) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(1, 0, myx);
setCrossCouplingErrors(m);
}
/**
* Gets y-z cross coupling error.
*
* @return y-z cross coupling error.
*/
public double getMyz() {
return crossCouplingErrors.getElementAt(1, 2);
}
/**
* Sets y-z cross coupling error.
*
* @param myz y-z cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non-invertible.
*/
public void setMyz(final double myz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(1, 2, myz);
setCrossCouplingErrors(m);
}
/**
* Gets z-x cross coupling error.
*
* @return z-x cross coupling error.
*/
public double getMzx() {
return crossCouplingErrors.getElementAt(2, 0);
}
/**
* Sets z-x cross coupling error.
*
* @param mzx z-x cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non-invertible.
*/
public void setMzx(final double mzx) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(2, 0, mzx);
setCrossCouplingErrors(m);
}
/**
* Gets z-y cross coupling error.
*
* @return z-y cross coupling error.
*/
public double getMzy() {
return crossCouplingErrors.getElementAt(2, 1);
}
/**
* Sets z-y cross coupling error.
*
* @param mzy z-y cross coupling error.
* @throws AlgebraException if provided value makes cross coupling matrix
* non-invertible.
*/
public void setMzy(final double mzy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(2, 1, mzy);
setCrossCouplingErrors(m);
}
/**
* Sets scaling factors.
*
* @param sx x scaling factor.
* @param sy y scaling factor.
* @param sz z scaling factor.
* @throws AlgebraException if provided values make cross coupling matrix
* non-invertible.
*/
public void setScalingFactors(
final double sx, final double sy, final double sz) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 0, sx);
m.setElementAt(1, 1, sy);
m.setElementAt(2, 2, sz);
setCrossCouplingErrors(m);
}
/**
* Sets cross coupling errors.
*
* @param mxy x-y cross coupling error.
* @param mxz x-z cross coupling error.
* @param myx y-x cross coupling error.
* @param myz y-z cross coupling error.
* @param mzx z-x cross coupling error.
* @param mzy z-y cross coupling error.
* @throws AlgebraException if provided values make cross coupling matrix
* non-invertible.
*/
public void setCrossCouplingErrors(
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 1, mxy);
m.setElementAt(0, 2, mxz);
m.setElementAt(1, 0, myx);
m.setElementAt(1, 2, myz);
m.setElementAt(2, 0, mzx);
m.setElementAt(2, 1, mzy);
setCrossCouplingErrors(m);
}
/**
* Sets scaling factors and cross coupling errors.
*
* @param sx x scaling factor.
* @param sy y scaling factor.
* @param sz z scaling factor.
* @param mxy x-y cross coupling error.
* @param mxz x-z cross coupling error.
* @param myx y-x cross coupling error.
* @param myz y-z cross coupling error.
* @param mzx z-x cross coupling error.
* @param mzy z-y cross coupling error.
* @throws AlgebraException if provided values make cross coupling matrix
* non-invertible.
*/
public void setScalingFactorsAndCrossCouplingErrors(
final double sx, final double sy, final double sz,
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy) throws AlgebraException {
final var m = new Matrix(Triad.COMPONENTS, Triad.COMPONENTS);
m.copyFrom(crossCouplingErrors);
m.setElementAt(0, 0, sx);
m.setElementAt(1, 1, sy);
m.setElementAt(2, 2, sz);
m.setElementAt(0, 1, mxy);
m.setElementAt(0, 2, mxz);
m.setElementAt(1, 0, myx);
m.setElementAt(1, 2, myz);
m.setElementAt(2, 0, mzx);
m.setElementAt(2, 1, mzy);
setCrossCouplingErrors(m);
}
/**
* Gets g-dependant cross biases matrix.
*
* @return g-dependant cross biases matrix.
*/
public Matrix getGDependantCrossBias() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4015 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2083 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1375 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1331 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2075 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3158 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3120 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known bias to be set
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known bias as a column matrix.
*
* @return known bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known bias as an array.
*
* @param bias bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2371 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2368 |
final var result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @return initial gyroscope bias to be used to find a solution as a
* column matrix.
*/
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* a column matrix.
* Values are expressed in radians per second (rad/s).
*
* @param initialBias initial gyroscope bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @return initial bias coordinates.
*/
public AngularSpeedTriad getInitialBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @param result instance where result will be stored.
*/
public void getInitialBiasAsTriad(AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial bias coordinates of gyroscope used to find a solution.
*
* @param initialBias initial bias coordinates to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setInitialBias(AngularSpeedTriad initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @return collection of sequences of timestamped body kinematics
* measurements.
*/
@Override
public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
return sequences;
}
/**
* Sets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @param sequences collection of sequences of timestamped body
* kinematics measurements.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setSequences(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences) throws LockedException {
if (running) {
throw new LockedException();
}
this.sequences = sequences;
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return true;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4346 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2302 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 743 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1411 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1367 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2464 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 728 |
initialBiasZ = convertAcceleration(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of initial bias.
*/
@Override
public double[] getInitialBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial bias to be used to find a solution as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* Values are expressed in meters per squared second (m/s^2).
*
* @return initial bias to be used to find a solution as a column matrix.
*/
@Override
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial bias to be used to find a solution as a column matrix.
* Values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial bias to be used to find a solution as a column matrix with
* values expressed in meters per squared second (m/s^2).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4028 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5378 |
setResult(m, b, g);
// at this point covariance is expressed in terms of b, M and G, and must
// be expressed in terms of bg, Mg and Gg.
// We know that:
// bg = M * b
// Mg = M - I
// Gg = M * G
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [m21 m22 m23]
// [m31 m32 m33]
// G = [g11 g12 g13]
// [g21 g22 g23]
// [g31 g32 g33]
// bg = [m11 m12 m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
// [m21 m22 m23][by] [m21 * bx + m22 * by + m23 * bz] [bgy]
// [m31 m32 m33][bz] [m31 * bx + m32 * by + m33 * bz] [bgz]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [m21 m22 - 1 m23 ]
// [mzx mzy sz ] [m31 m32 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = [m11 m12 m13][g11 g12 g13]
// [gg21 gg22 gg23] [m21 m22 m23][g21 g22 g23]
// [gg31 gg32 gg33] [m31 m32 m33][g31 g32 g33]
// Defining the linear application:
// F(b, M, G) = F(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
// as:
// [bgx] = [m11 * bx + m12 * by + m13 * bz]
// [bgy] [m21 * bx + m22 * by + m23 * bz]
// [bgz] [m31 * bx + m32 * by + m33 * bz]
// [sx] [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [m21]
// [myz] [m23]
// [mzx] [m31]
// [mzy] [m32]
// [gg11] [m11 * g11 + m12 * g21 + m13 * g31]
// [gg21] [m21 * g11 + m22 * g21 + m23 * g31]
// [gg31] [m31 * g11 + m32 * g21 + m33 * g31]
// [gg12] [m11 * g12 + m12 * g22 + m13 * g32]
// [gg22] [m21 * g12 + m22 * g22 + m23 * g32]
// [gg32] [m31 * g12 + m32 * g22 + m33 * g32]
// [gg13] [m11 * g13 + m12 * g23 + m13 * g33]
// [gg23] [m21 * g13 + m22 * g23 + m23 * g33]
// [gg33] [m31 * g13 + m32 * g23 + m33 * g33]
// Then the Jacobian of F(b, M, G) is:
// J = [m11 m12 m13 bx 0 0 by 0 0 bz 0 0 0 0 0 0 0 0 0 0 0 ]
// [m21 m22 m23 0 bx 0 0 by 0 0 bz 0 0 0 0 0 0 0 0 0 0 ]
// [m31 m32 m33 0 0 bx 0 0 by 0 0 bz 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 g11 0 0 g21 0 0 g31 0 0 m11 m12 m13 0 0 0 0 0 0 ]
// [0 0 0 0 g11 0 0 g21 0 0 g31 0 m21 m22 m23 0 0 0 0 0 0 ]
// [0 0 0 0 0 g11 0 0 g21 0 0 g31 m31 m32 m33 0 0 0 0 0 0 ]
// [0 0 0 g12 0 0 g22 0 0 g32 0 0 0 0 0 m11 m12 m13 0 0 0 ]
// [0 0 0 0 g12 0 0 g22 0 0 g32 0 0 0 0 m21 m22 m23 0 0 0 ]
// [0 0 0 0 0 g12 0 0 g22 0 0 g32 0 0 0 m31 m32 m33 0 0 0 ]
// [0 0 0 g13 0 0 g23 0 0 g33 0 0 0 0 0 0 0 0 m11 m12 m13]
// [0 0 0 0 g13 0 0 g23 0 0 g33 0 0 0 0 0 0 0 m21 m22 m23]
// [0 0 0 0 0 g13 0 0 g23 0 0 g33 0 0 0 0 0 0 m31 m32 m33]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, GENERAL_UNKNOWNS_AND_CROSS_BIASES);
jacobian.setElementAt(0, 0, m11);
jacobian.setElementAt(0, 1, m12);
jacobian.setElementAt(0, 2, m13);
jacobian.setElementAt(0, 3, bx);
jacobian.setElementAt(0, 6, by);
jacobian.setElementAt(0, 9, bz);
jacobian.setElementAt(1, 0, m21);
jacobian.setElementAt(1, 1, m22);
jacobian.setElementAt(1, 2, m23);
jacobian.setElementAt(1, 4, bx);
jacobian.setElementAt(1, 7, by);
jacobian.setElementAt(1, 10, bz);
jacobian.setElementAt(2, 0, m31);
jacobian.setElementAt(2, 1, m32);
jacobian.setElementAt(2, 2, m33);
jacobian.setElementAt(2, 5, bx);
jacobian.setElementAt(2, 8, by);
jacobian.setElementAt(2, 11, bz);
jacobian.setElementAt(3, 3, 1.0);
jacobian.setElementAt(4, 7, 1.0);
jacobian.setElementAt(5, 11, 1.0);
jacobian.setElementAt(6, 6, 1.0);
jacobian.setElementAt(7, 9, 1.0);
jacobian.setElementAt(8, 4, 1.0);
jacobian.setElementAt(9, 10, 1.0);
jacobian.setElementAt(10, 5, 1.0);
jacobian.setElementAt(11, 8, 1.9);
jacobian.setElementAt(12, 3, g11);
jacobian.setElementAt(12, 6, g21);
jacobian.setElementAt(12, 9, g31);
jacobian.setElementAt(12, 12, m11);
jacobian.setElementAt(12, 13, m12);
jacobian.setElementAt(12, 14, m13);
jacobian.setElementAt(13, 4, g11);
jacobian.setElementAt(13, 7, g21);
jacobian.setElementAt(13, 10, g31);
jacobian.setElementAt(13, 12, m21);
jacobian.setElementAt(13, 13, m22);
jacobian.setElementAt(13, 14, m23);
jacobian.setElementAt(14, 5, g11);
jacobian.setElementAt(14, 8, g21);
jacobian.setElementAt(14, 11, g31);
jacobian.setElementAt(14, 12, m31);
jacobian.setElementAt(14, 13, m32);
jacobian.setElementAt(14, 14, m33);
jacobian.setElementAt(15, 3, g12);
jacobian.setElementAt(15, 6, g22);
jacobian.setElementAt(15, 9, g32);
jacobian.setElementAt(15, 15, m11);
jacobian.setElementAt(15, 16, m12);
jacobian.setElementAt(15, 17, m13);
jacobian.setElementAt(16, 4, g12);
jacobian.setElementAt(16, 7, g22);
jacobian.setElementAt(16, 10, g32);
jacobian.setElementAt(16, 15, m21);
jacobian.setElementAt(16, 16, m22);
jacobian.setElementAt(16, 17, m23);
jacobian.setElementAt(17, 5, g12);
jacobian.setElementAt(17, 8, g22);
jacobian.setElementAt(17, 11, g32);
jacobian.setElementAt(17, 15, m31);
jacobian.setElementAt(17, 16, m32);
jacobian.setElementAt(17, 17, m33);
jacobian.setElementAt(18, 3, g13);
jacobian.setElementAt(18, 6, g23);
jacobian.setElementAt(18, 9, g33);
jacobian.setElementAt(18, 18, m11);
jacobian.setElementAt(18, 19, m12);
jacobian.setElementAt(18, 20, m13);
jacobian.setElementAt(19, 4, g13);
jacobian.setElementAt(19, 7, g23);
jacobian.setElementAt(19, 10, g33);
jacobian.setElementAt(19, 18, m21);
jacobian.setElementAt(19, 19, m22);
jacobian.setElementAt(19, 20, m23);
jacobian.setElementAt(20, 5, g13);
jacobian.setElementAt(20, 8, g23);
jacobian.setElementAt(20, 11, g33);
jacobian.setElementAt(20, 18, m31);
jacobian.setElementAt(20, 19, m32);
jacobian.setElementAt(20, 20, m33);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Internal method to perform calibration when common z-axis is assumed
* for both the accelerometer and gyroscope and G-dependent cross biases
* are ignored.
*
* @throws AlgebraException if accelerometer parameters prevent fixing
* measured accelerometer values due to numerical instabilities.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateCommonAxis() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5354 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6092 |
jacobian.setElementAt(8, 5, 1.9);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Sets input data into Levenberg-Marquardt fitter when G-dependent cross biases
* are taken into account.
*
* @throws AlgebraException if provided accelerometer cross coupling
* errors are not valid.
* @throws InvalidSourceAndDestinationFrameTypeException never happens
*/
private void setInputDataWithGDependentCrossBiases()
throws AlgebraException, InvalidSourceAndDestinationFrameTypeException {
// compute reference frame at current position
final var nedPosition = getNedPosition();
final var nedC = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
final var nedFrame = new NEDFrame(nedPosition, nedC);
final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
final var refKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
ecefFrame);
final var refAngularRateX = refKinematics.getAngularRateX();
final var refAngularRateY = refKinematics.getAngularRateY();
final var refAngularRateZ = refKinematics.getAngularRateZ();
final var w2 = turntableRotationRate * turntableRotationRate;
final var numMeasurements = measurements.size();
final var x = new Matrix(numMeasurements, 2 * BodyKinematics.COMPONENTS);
final var y = new double[numMeasurements];
final var angularRateStandardDeviations = new double[numMeasurements];
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var angularRateX = measuredKinematics.getAngularRateX();
final var angularRateY = measuredKinematics.getAngularRateY();
final var angularRateZ = measuredKinematics.getAngularRateZ();
final var fX = measuredKinematics.getFx();
final var fY = measuredKinematics.getFy();
final var fZ = measuredKinematics.getFz();
x.setElementAt(i, 0, angularRateX - refAngularRateX);
x.setElementAt(i, 1, angularRateY - refAngularRateY);
x.setElementAt(i, 2, angularRateZ - refAngularRateZ);
x.setElementAt(i, 3, fX);
x.setElementAt(i, 4, fY);
x.setElementAt(i, 5, fZ);
y[i] = w2;
angularRateStandardDeviations[i] = measurement.getAngularRateStandardDeviation();
i++;
}
fitter.setInputData(x, y, angularRateStandardDeviations);
ba = getAccelerometerBiasAsMatrix();
ma = getAccelerometerMa();
accelerationFixer.setBias(ba);
accelerationFixer.setCrossCouplingErrors(ma);
}
/**
* Sets input data into Levenberg-Marquardt fitter when G-dependent cross biases
* are ignored.
*
* @throws AlgebraException if provided accelerometer cross coupling
* errors are not valid.
* @throws InvalidSourceAndDestinationFrameTypeException never happens.
*/
private void setInputData() throws AlgebraException, InvalidSourceAndDestinationFrameTypeException {
// compute reference frame at current position
final var nedPosition = getNedPosition();
final var nedC = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
final var nedFrame = new NEDFrame(nedPosition, nedC);
final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
final var refKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
ecefFrame);
final var refAngularRateX = refKinematics.getAngularRateX();
final var refAngularRateY = refKinematics.getAngularRateY();
final var refAngularRateZ = refKinematics.getAngularRateZ();
final var w2 = turntableRotationRate * turntableRotationRate;
final var numMeasurements = measurements.size();
final var x = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
final var y = new double[numMeasurements];
final var angularRateStandardDeviations = new double[numMeasurements];
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var angularRateX = measuredKinematics.getAngularRateX();
final var angularRateY = measuredKinematics.getAngularRateY();
final var angularRateZ = measuredKinematics.getAngularRateZ();
x.setElementAt(i, 0, angularRateX - refAngularRateX);
x.setElementAt(i, 1, angularRateY - refAngularRateY);
x.setElementAt(i, 2, angularRateZ - refAngularRateZ);
y[i] = w2;
angularRateStandardDeviations[i] = measurement.getAngularRateStandardDeviation();
i++;
}
fitter.setInputData(x, y, angularRateStandardDeviations);
ba = getAccelerometerBiasAsMatrix();
ma = getAccelerometerMa();
accelerationFixer.setBias(ba);
accelerationFixer.setCrossCouplingErrors(ma);
}
/**
* Converts provided NED position expressed in terms of latitude, longitude and height respect
* mean Earth surface, to position expressed in ECEF coordinates.
*
* @param position NED position to be converted.
* @return converted position expressed in ECEF coordinates.
*/
private static ECEFPosition convertPosition(final NEDPosition position) {
final var velocity = new ECEFVelocity();
final var result = new ECEFPosition();
NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
position.getLatitude(), position.getLongitude(), position.getHeight(), 0.0, 0.0, 0.0,
result, velocity);
return result;
}
/**
* Converts acceleration instance to meters per squared second.
*
* @param acceleration acceleration instance to be converted.
* @return converted value.
*/
private static double convertAcceleration(final Acceleration acceleration) {
return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Converts angular speed instance to radians per second (rad/s).
*
* @param value angular speed value.
* @param unit unit of angular speed value.
* @return converted value.
*/
private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Converts angular speed instance to radians per second (rad/s).
*
* @param angularSpeed angular speed instance to be converted.
* @return converted value.
*/
private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
}
/**
* Converts time instance to seconds.
*
* @param time time instance to be converted.
* @return converted value.
*/
private static double convertTime(final Time time) {
return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
}
/**
* Makes proper conversion of internal cross-coupling and g-dependent
* cross bias matrices.
*
* @param m internal scaling and cross-coupling matrix.
* @param g internal g-dependent cross bias matrix.
* @throws AlgebraException if a numerical instability occurs.
*/
private void setResult(final Matrix m, final Matrix g) throws AlgebraException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4015 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2083 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1375 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1331 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2079 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known bias to be set
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known bias as a column matrix.
*
* @return known bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known bias as an array.
*
* @param bias bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1266 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1285 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetBias(bias);
}
/**
* Gets known accelerometer bias as a column matrix.
*
* @return known accelerometer bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known accelerometer bias as a column matrix.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetBias(bias);
}
/**
* Gets initial x scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java | 250 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java | 335 |
}
}
/**
* Gets estimated average of x coordinate of measurement expressed in its default
* unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
* This value will depend of body location and orientation, hence it should never
* be used as a calibration bias.
*
* @return average of x coordinate of measurement in current window.
*/
public double getAvgX() {
return avgX;
}
/**
* Gets estimated average of x coordinate of measurement within current window.
* This value will depend of body location and orientation, hence it should never
* be used as a calibration bias.
*
* @return average of x coordinate of measurement in current window.
*/
public M getAvgXAsMeasurement() {
return createMeasurement(avgX, getDefaultUnit());
}
/**
* Gets estimated average of x coordinate of measurement within current window.
* This value will depend of body location and orientation, hence it should never
* be used as a calibration bias.
*
* @param result instance where average of x coordinate of measurement will be stored.
*/
public void getAvgXAsMeasurement(final M result) {
result.setValue(avgX);
result.setUnit(getDefaultUnit());
}
/**
* Gets estimated average of y coordinate of measurement expressed in its default
* unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
* This value will depend of body location and orientation, hence it should never
* be used as a calibration bias.
*
* @return average of y coordinate of measurement in current window.
*/
public double getAvgY() {
return avgY;
}
/**
* Gets estimated average of y coordinate of measurement within current window.
* This value will depend of body location and orientation, hence it should never
* be used as a calibration bias.
*
* @return average of y coordinate of measurement in current window.
*/
public M getAvgYAsMeasurement() {
return createMeasurement(avgY, getDefaultUnit());
}
/**
* Gets estimated average of y coordinate of measurement within current window.
* This value will depend of body location and orientation, hence it should never
* be used as a calibration bias.
*
* @param result instance where average of y coordinate of measurement will be stored.
*/
public void getAvgYAsMeasurement(final M result) {
result.setValue(avgY);
result.setUnit(getDefaultUnit());
}
/**
* Gets estimated average of z coordinate of measurement expressed in its default
* unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
* This value will depend of body location and orientation, hence it should never
* be used as a calibration bias.
*
* @return average of z coordinate of measurement in current window.
*/
public double getAvgZ() {
return avgZ;
}
/**
* Gets estimated average of z coordinate of measurement within current window.
* This value will depend of body location and orientation, hence it should never
* be used as a calibration bias.
*
* @return average of z coordinate of measurement in current window.
*/
public M getAvgZAsMeasurement() {
return createMeasurement(avgZ, getDefaultUnit());
}
/**
* Gets estimated average of z coordinate of measurement within current window.
* This value will depend of body location and orientation, hence it should never
* be used as a calibration bias.
*
* @param result instance where average of z coordinate of measurement will be stored.
*/
public void getAvgZAsMeasurement(final M result) {
result.setValue(avgZ);
result.setUnit(getDefaultUnit());
}
/**
* Gets estimated average as a measurement triad.
*
* @return average measurement triad.
*/
public T getAvgTriad() {
return createTriad(avgX, avgY, avgZ, getDefaultUnit());
}
/**
* Gets estimated average as a measurement triad.
*
* @param result instance where average values and unit will be stored.
*/
public void getAvgTriad(final T result) {
result.setValueCoordinatesAndUnit(avgX, avgY, avgZ, getDefaultUnit());
}
/**
* Gets norm of estimated average measurement expressed in its default
* unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
* This value is independent of body orientation.
*
* @return norm of estimated average specific force.
*/
public double getAvgNorm() {
return Math.sqrt(avgX * avgX + avgY * avgY + avgZ * avgZ);
}
/**
* Gets norm of estimated average measurement within current window.
*
* @return norm of estimated average measurement.
*/
public M getAvgNormAsMeasurement() {
return createMeasurement(getAvgNorm(), getDefaultUnit());
}
/**
* Gets norm of estimated average measurement within current window.
*
* @param result instance where norm of estimated average measurement will be stored.
*/
public void getAvgNormAsMeasurement(final M result) {
result.setValue(getAvgNorm());
result.setUnit(getDefaultUnit());
}
/**
* Gets estimated variance of x coordinate of measurement within current window
* expressed in its default squared unit (m^2/s^4 for acceleration,
* rad^2/s^2 for angular speed or T^2 for magnetic flux density).
*
* @return estimated variance of x coordinate of measurement within current
* window.
*/
public double getVarianceX() {
return varianceX;
}
/**
* Gets estimated variance of y coordinate of measurement within current window
* expressed in its default squared unit (m^2/s^4 for acceleration,
* rad^2/s^2 for angular speed or T^2 for magnetic flux density).
*
* @return estimated variance of y coordinate of measurement within current
* window.
*/
public double getVarianceY() {
return varianceY;
}
/**
* Gets estimated variance of z coordinate of measurement within current window
* expressed in its default squared unit (m^2/s^4 for acceleration,
* rad^2/s^2 for angular speed or T^2 for magnetic flux density).
*
* @return estimated variance of z coordinate of measurement within current
* window.
*/
public double getVarianceZ() {
return varianceZ;
}
/**
* Gets estimated standard deviation of x coordinate of measurement within current
* window and expressed in its default unit (m/s^2 for acceleration, rad/s for
* angular speed or T for magnetic flux density).
*
* @return estimated standard deviation of x coordinate of measurement within
* current window.
*/
public double getStandardDeviationX() {
return Math.sqrt(varianceX);
}
/**
* Gets estimated standard deviation of x coordinate of measurement within current
* window.
*
* @return estimated standard deviation of x coordinate of measurement.
*/
public M getStandardDeviationXAsMeasurement() {
return createMeasurement(getStandardDeviationX(), getDefaultUnit());
}
/**
* Gets estimated standard deviation of x coordinate of measurement within current
* window.
*
* @param result instance where estimated standard deviation of x coordinate of
* measurement will be stored.
*/
public void getStandardDeviationXAsMeasurement(final M result) {
result.setValue(getStandardDeviationX());
result.setUnit(getDefaultUnit());
}
/**
* Gets estimated standard deviation of y coordinate of measurement within current
* window and expressed in its default unit (m/s^2 for acceleration, rad/s for
* angular speed or T for magnetic flux density).
*
* @return estimated standard deviation of y coordinate of measurement within
* current window.
*/
public double getStandardDeviationY() {
return Math.sqrt(varianceY);
}
/**
* Gets estimated standard deviation of y coordinate of measurement within current
* window.
*
* @return estimated standard deviation of y coordinate of measurement.
*/
public M getStandardDeviationYAsMeasurement() {
return createMeasurement(getStandardDeviationY(), getDefaultUnit());
}
/**
* Gets estimated standard deviation of y coordinate of measurement within current
* window.
*
* @param result instance where estimated standard deviation of y coordinate of
* measurement will be stored.
*/
public void getStandardDeviationYAsMeasurement(final M result) {
result.setValue(getStandardDeviationY());
result.setUnit(getDefaultUnit());
}
/**
* Gets estimated standard deviation of z coordinate of measurement within current
* window and expressed in its default unit (m/s^2 for acceleration, rad/s for
* angular speed or T for magnetic flux density).
*
* @return estimated standard deviation of z coordinate of measurement within
* current window.
*/
public double getStandardDeviationZ() {
return Math.sqrt(varianceZ);
}
/**
* Gets estimated standard deviation of z coordinate of measurement within current
* window.
*
* @return estimated standard deviation of z coordinate of measurement.
*/
public M getStandardDeviationZAsMeasurement() {
return createMeasurement(getStandardDeviationZ(), getDefaultUnit());
}
/**
* Gets estimated standard deviation of z coordinate of measurement within current
* window.
*
* @param result instance where estimated standard deviation of z coordinate of
* measurement will be stored.
*/
public void getStandardDeviationZAsMeasurement(final M result) {
result.setValue(getStandardDeviationZ());
result.setUnit(getDefaultUnit());
}
/**
* Gets estimated standard deviation of measurements.
*
* @return estimated standard deviation triad of measurements.
*/
public T getStandardDeviationTriad() {
return createTriad(getStandardDeviationX(), getStandardDeviationY(), getStandardDeviationZ(), getDefaultUnit());
}
/**
* Gets estimated standard deviation of measurement within current window.
*
* @param result instance where estimated standard deviation triad of
* measurement will be stored.
*/
public void getStandardDeviationTriad(final T result) {
result.setValueCoordinatesAndUnit(getStandardDeviationX(), getStandardDeviationY(), getStandardDeviationZ(),
getDefaultUnit());
}
/**
* Gets norm of estimated standard deviation of measurement
* expressed in its default unit (m/s^2 for acceleration, rad/s for
* angular speed or T for magnetic flux density).
*
* @return norm of estimated standard deviation of measurement.
*/
public double getStandardDeviationNorm() {
final var fx = getStandardDeviationX();
final var fy = getStandardDeviationY();
final var fz = getStandardDeviationZ();
return Math.sqrt(fx * fx + fy * fy + fz * fz);
}
/**
* Gets norm of estimated standard deviation of measurements.
*
* @return norm of estimated standard deviation of measurement.
*/
public M getStandardDeviationNormAsMeasurement() {
return createMeasurement(getStandardDeviationNorm(), getDefaultUnit());
}
/**
* Gets norm of estimated standard deviation of measurement within current window.
*
* @param result instance where norm of estimated standard deviation will be stored.
*/
public void getStandardDeviationNormAsMeasurement(final M result) {
result.setValue(getStandardDeviationNorm());
result.setUnit(getDefaultUnit());
}
/**
* Gets average of estimated standard deviation coordinates of measurement
* expressed in its default unit (m/s^2 for acceleration, rad/s for
* angular speed or T for magnetic flux density).
*
* @return average of estimated standard deviation coordinates.
*/
public double getAverageStandardDeviation() {
final var fx = getStandardDeviationX();
final var fy = getStandardDeviationY();
final var fz = getStandardDeviationZ();
return (fx + fy + fz) / 3.0;
}
/**
* Gets average of estimated standard deviation coordinates of measurement within
* current window.
*
* @return average of estimated standard deviation coordinates.
*/
public M getAverageStandardDeviationAsMeasurement() {
return createMeasurement(getAverageStandardDeviation(), getDefaultUnit());
}
/**
* Gets average of estimated standard deviation coordinates of measurement.
*
* @param result instance where average of estimated standard deviation coordinates
* will be stored.
*/
public void getAverageStandardDeviationAsMeasurement(final M result) {
result.setValue(getAverageStandardDeviation());
result.setUnit(getDefaultUnit());
}
/**
* Gets measurement noise PSD (Power Spectral Density) on x axis expressed
* in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
* magnetometer.
*
* @return measurement noise PSD on x axis.
*/
public double getPsdX() {
return varianceX * timeInterval;
}
/**
* Gets measurement noise PSD (Power Spectral Density) on y axis expressed
* in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
* magnetometer.
*
* @return measurement noise PSD on y axis.
*/
public double getPsdY() {
return varianceY * timeInterval;
}
/**
* Gets measurement noise PSD (Power Spectral Density) on z axis expressed
* in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
* magnetometer.
*
* @return measurement noise PSD on z axis.
*/
public double getPsdZ() {
return varianceZ * timeInterval;
}
/**
* Gets measurement noise root PSD (Power Spectral Density) on x axis expressed in
* (m * s^-1.5) for accelerometer, (rad * s^-0.5) for gyroscope or (T * s^0.5) for
* magnetometer.
*
* @return measurement noise root PSD on x axis.
*/
public double getRootPsdX() {
return Math.sqrt(getPsdX());
}
/**
* Gets measurement noise root PSD (Power Spectral Density) on y axis expressed in
* (m * s^-1.5) for accelerometer, (rad * s^-0.5) for gyroscope or (T * s^0.5) for
* magnetometer.
*
* @return measurement noise root PSD on y axis.
*/
public double getRootPsdY() {
return Math.sqrt(getPsdY());
}
/**
* Gets measurement noise root PSD (Power Spectral Density) on z axis expressed in
* (m * s^-1.5) for accelerometer, (rad * s^-0.5) for gyroscope or (T * s^0.5) for
* magnetometer.
*
* @return measurement noise root PSD on z axis.
*/
public double getRootPsdZ() {
return Math.sqrt(getPsdZ());
}
/**
* Gets average measurement noise PSD (Power Spectral Density) among
* x,y,z components expressed as (m^2 * s^-3) for accelerometer,
* (rad^2/s) for gyroscope or (T^2 * s) for magnetometer.
*
* @return average measurement noise PSD.
*/
public double getAvgNoisePsd() {
return (getPsdX() + getPsdY() + getPsdZ()) / 3.0;
}
/**
* Gets norm of noise root PSD (Power Spectral Density) among x,y,z
* components expressed as (m * s^-1.5) for accelerometer,
* (rad * s^-0.5) for gyroscope or (T * s^0.5) for magnetometer.
*
* @return norm of measurement noise root PSD.
*/
public double getNoiseRootPsdNorm() {
return Math.sqrt(getPsdX() + getPsdY() + getPsdZ());
}
/**
* Gets number of samples that have been processed so far.
*
* @return number of samples that have been processed so far.
*/
public int getNumberOfProcessedSamples() {
return numberOfProcessedSamples;
}
/**
* Indicates whether estimator is currently running or not.
*
* @return true if estimator is running, false otherwise.
*/
public boolean isRunning() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3135 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4434 |
setInputData();
fitter.fit();
final var result = fitter.getA();
final var m11 = result[0];
final var m12 = result[1];
final var m22 = result[2];
final var m13 = result[3];
final var m23 = result[4];
final var m33 = result[5];
final var g11 = result[6];
final var g21 = result[7];
final var g31 = result[8];
final var g12 = result[9];
final var g22 = result[10];
final var g32 = result[11];
final var g13 = result[12];
final var g23 = result[13];
final var g33 = result[14];
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mm.setElementAtIndex(0, m11);
mm.setElementAtIndex(1, 0.0);
mm.setElementAtIndex(2, 0.0);
mm.setElementAtIndex(3, m12);
mm.setElementAtIndex(4, m22);
mm.setElementAtIndex(5, 0.0);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33);
final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mg.setElementAtIndex(0, g11);
mg.setElementAtIndex(1, g21);
mg.setElementAtIndex(2, g31);
mg.setElementAtIndex(3, g12);
mg.setElementAtIndex(4, g22);
mg.setElementAtIndex(5, g32);
mg.setElementAtIndex(6, g13);
mg.setElementAtIndex(7, g23);
mg.setElementAtIndex(8, g33);
setResult(mm, mg);
// at this point covariance is expressed in terms of M and G, and must
// be expressed in terms of Mg and Gg.
// We know that:
// Mg = M - I
// Gg = M * G
// M = [m11 m12 m13]
// [0 m22 m23]
// [0 0 m33]
// G = [g11 g12 g13]
// [g21 g22 g23]
// [g31 g32 g33]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [0 m22 - 1 m23 ]
// [mzx mzy sz ] [0 0 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = [m11 m12 m13][g11 g12 g13]
// [gg21 gg22 gg23] [0 m22 m23][g21 g22 g23]
// [gg31 gg32 gg33] [0 0 m33][g31 g32 g33]
// Defining the linear application:
// F(M, G) = F(m11, m12, m22, m32=0, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
// as:
// [sx] = [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [0]
// [myz] [m23]
// [mzx] [0]
// [mzy] [0]
// [gg11] [m11 * g11 + m12 * g21 + m13 * g31]
// [gg21] [ m22 * g21 + m23 * g31]
// [gg31] [ m33 * g31]
// [gg12] [m11 * g12 + m12 * g22 + m13 * g32]
// [gg22] [ m22 * g22 + n23 * g32]
// [gg32] [ m33 * g32]
// [gg13] [m11 * g13 + m12 * g23 + m13 * g33]
// [gg23] [ m22 * g23 + m23 * g33]
// [gg33] [ m33 * g33]
// Then the Jacobian of F(M, G) is:
// J = [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 ]
// [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [g11 g21 0 g31 0 0 m11 m12 m13 0 0 0 0 0 0 ]
// [0 0 g21 0 g31 0 0 m22 m23 0 0 0 0 0 0 ]
// [0 0 0 0 0 g31 0 0 m33 0 0 0 0 0 0 ]
// [g12 g22 0 g32 0 0 0 0 0 m11 m12 m13 0 0 0 ]
// [0 0 g22 0 g32 0 0 0 0 0 m22 m23 0 0 0 ]
// [0 0 0 0 0 g32 0 0 0 0 0 m33 0 0 0 ]
// [g13 g23 0 g33 0 0 0 0 0 0 0 0 m11 m12 m13]
// [0 0 g23 0 g33 0 0 0 0 0 0 0 0 m22 m23]
// [0 0 0 0 0 g33 0 0 0 0 0 0 0 0 m33]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES);
jacobian.setElementAt(0, 0, 1.0);
jacobian.setElementAt(1, 2, 1.0);
jacobian.setElementAt(2, 5, 1.0);
jacobian.setElementAt(3, 1, 1.0);
jacobian.setElementAt(4, 3, 1.0);
jacobian.setElementAt(6, 4, 1.0);
jacobian.setElementAt(9, 0, g11);
jacobian.setElementAt(9, 1, g21);
jacobian.setElementAt(9, 3, g31);
jacobian.setElementAt(9, 6, m11);
jacobian.setElementAt(9, 7, m12);
jacobian.setElementAt(9, 8, m13);
jacobian.setElementAt(10, 2, g21);
jacobian.setElementAt(10, 4, g31);
jacobian.setElementAt(10, 7, m22);
jacobian.setElementAt(10, 8, m23);
jacobian.setElementAt(11, 5, g31);
jacobian.setElementAt(11, 8, m33);
jacobian.setElementAt(12, 0, g12);
jacobian.setElementAt(12, 1, g22);
jacobian.setElementAt(12, 3, g32);
jacobian.setElementAt(12, 9, m11);
jacobian.setElementAt(12, 10, m12);
jacobian.setElementAt(12, 11, m13);
jacobian.setElementAt(13, 2, g22);
jacobian.setElementAt(13, 4, g32);
jacobian.setElementAt(13, 10, m22);
jacobian.setElementAt(13, 11, m23);
jacobian.setElementAt(14, 5, g32);
jacobian.setElementAt(14, 11, m33);
jacobian.setElementAt(15, 0, g13);
jacobian.setElementAt(15, 1, g23);
jacobian.setElementAt(15, 3, g33);
jacobian.setElementAt(15, 12, m11);
jacobian.setElementAt(15, 13, m12);
jacobian.setElementAt(15, 14, m13);
jacobian.setElementAt(16, 2, g23);
jacobian.setElementAt(16, 4, g33);
jacobian.setElementAt(16, 13, m22);
jacobian.setElementAt(16, 14, m23);
jacobian.setElementAt(17, 5, g33);
jacobian.setElementAt(17, 14, m33);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Internal method to perform general calibration when G-dependent cross
* biases are being estimated.
*
* @throws AlgebraException if accelerometer parameters prevent fixing
* measured accelerometer values due to numerical instabilities.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateGeneralAndGDependentCrossBiases() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2865 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 480 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3225 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4157 |
} catch (final FittingException | AlgebraException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where estimated gyroscope biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return column matrix containing x,y,z components of estimated gyroscope
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasX() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasY() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasZ() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedX() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedY() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedZ() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope bias.
*
* @return estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeedTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if estimated gyroscope bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2870 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3230 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2982 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4162 |
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where estimated gyroscope biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return column matrix containing x,y,z components of estimated gyroscope
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasX() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasY() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasZ() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedX() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedY() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedZ() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope bias.
*
* @return estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeedTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if estimated gyroscope bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 634 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3717 |
} catch (final AlgebraException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @return array containing x,y,z components of estimated magnetometer
* hard-iron biases.
*/
@Override
public double[] getEstimatedHardIron() {
return estimatedHardIron;
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @param result instance where estimated magnetometer biases will be
* stored.
* @return true if result instance was updated, false otherwise (when
* estimation is not yet available).
*/
@Override
public boolean getEstimatedHardIron(final double[] result) {
if (estimatedHardIron != null) {
System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @return column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases.
*/
@Override
public Matrix getEstimatedHardIronAsMatrix() {
return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedHardIron != null) {
result.fromArray(estimatedHardIron);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return x coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronX() {
return estimatedHardIron != null ? estimatedHardIron[0] : null;
}
/**
* Gets y coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return y coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronY() {
return estimatedHardIron != null ? estimatedHardIron[1] : null;
}
/**
* Gets z coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return z coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronZ() {
return estimatedHardIron != null ? estimatedHardIron[2] : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @return x coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[0]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @return y coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[1]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @return z coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[2]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer bias.
*
* @return estimated magnetometer bias or null if not available.
*/
@Override
public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
return estimatedHardIron != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2]) : null;
}
/**
* Gets estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
if (estimatedHardIron != null) {
result.setValueCoordinatesAndUnit(
estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2], MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5002 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 468 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2977 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFy() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFz() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFxAsAcceleration() {
return estimatedBiases != null ?
new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFyAsAcceleration() {
return estimatedBiases != null ?
new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFzAsAcceleration() {
return estimatedBiases != null ?
new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer bias.
*
* @return estimated accelerometer bias or null if not available.
*/
@Override
public AccelerationTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null ?
new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2]) : null;
}
/**
* Gets estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated accelerometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2141 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 634 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @return array containing x,y,z components of estimated magnetometer
* hard-iron biases.
*/
@Override
public double[] getEstimatedHardIron() {
return estimatedHardIron;
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @param result instance where estimated magnetometer biases will be
* stored.
* @return true if result instance was updated, false otherwise (when
* estimation is not yet available).
*/
@Override
public boolean getEstimatedHardIron(final double[] result) {
if (estimatedHardIron != null) {
System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @return column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases.
*/
@Override
public Matrix getEstimatedHardIronAsMatrix() {
return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedHardIron != null) {
result.fromArray(estimatedHardIron);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return x coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronX() {
return estimatedHardIron != null ? estimatedHardIron[0] : null;
}
/**
* Gets y coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return y coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronY() {
return estimatedHardIron != null ? estimatedHardIron[1] : null;
}
/**
* Gets z coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return z coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronZ() {
return estimatedHardIron != null ? estimatedHardIron[2] : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @return x coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[0]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @return y coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[1]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @return z coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[2]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer bias.
*
* @return estimated magnetometer bias or null if not available.
*/
@Override
public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
return estimatedHardIron != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2])
: null;
}
/**
* Gets estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
if (estimatedHardIron != null) {
result.setValueCoordinatesAndUnit(estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2],
MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2870 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3230 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1693 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4243 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4162 |
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where estimated gyroscope biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return column matrix containing x,y,z components of estimated gyroscope
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasX() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasY() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated gyroscope bias expressed in radians per
* second (rad/s).
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasZ() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedX() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedY() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedZ() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope bias.
*
* @return estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeedTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if estimated gyroscope bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 485 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2982 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1693 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4243 |
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where estimated gyroscope biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return column matrix containing x,y,z component of estimated gyroscope
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated gyroscope bias expressed in radians per second
* (rad/s).
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasX() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated gyroscope bias expressed in radians per second
* (rad/s).
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasY() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated gyroscope bias expressed in radians per second
* (rad/s).
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public Double getEstimatedBiasZ() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @return x coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedX() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[0], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets x coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedX(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @return y coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedY() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[1], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets y coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedY(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @return z coordinate of estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeed getEstimatedBiasAngularSpeedZ() {
return estimatedBiases != null
? new AngularSpeed(estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets z coordinate of estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasAngularSpeedZ(final AngularSpeed result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope bias.
*
* @return estimated gyroscope bias or null if not available.
*/
@Override
public AngularSpeedTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if estimated gyroscope bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AngularSpeedTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2], AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5007 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2982 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1632 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2290 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2249 |
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFy() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFz() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFxAsAcceleration() {
return estimatedBiases != null ?
new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFyAsAcceleration() {
return estimatedBiases != null ?
new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFzAsAcceleration() {
return estimatedBiases != null ?
new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer bias.
*
* @return estimated accelerometer bias or null if not available.
*/
@Override
public AccelerationTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null ?
new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2]) : null;
}
/**
* Gets estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated accelerometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2146 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3722 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1658 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2379 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2512 |
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @return array containing x,y,z components of estimated magnetometer
* hard-iron biases.
*/
@Override
public double[] getEstimatedHardIron() {
return estimatedHardIron;
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @param result instance where estimated magnetometer biases will be
* stored.
* @return true if result instance was updated, false otherwise (when
* estimation is not yet available).
*/
@Override
public boolean getEstimatedHardIron(final double[] result) {
if (estimatedHardIron != null) {
System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @return column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases.
*/
@Override
public Matrix getEstimatedHardIronAsMatrix() {
return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedHardIron != null) {
result.fromArray(estimatedHardIron);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return x coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronX() {
return estimatedHardIron != null ? estimatedHardIron[0] : null;
}
/**
* Gets y coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return y coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronY() {
return estimatedHardIron != null ? estimatedHardIron[1] : null;
}
/**
* Gets z coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return z coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronZ() {
return estimatedHardIron != null ? estimatedHardIron[2] : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @return x coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[0]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @return y coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[1]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @return z coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[2]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer bias.
*
* @return estimated magnetometer bias or null if not available.
*/
@Override
public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
return estimatedHardIron != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2])
: null;
}
/**
* Gets estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
if (estimatedHardIron != null) {
result.setValueCoordinatesAndUnit(estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2],
MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 473 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1632 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2290 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2249 |
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() {
return estimatedBiases != null ? estimatedBiases[0] : null;
}
/**
* Gets y coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFy() {
return estimatedBiases != null ? estimatedBiases[1] : null;
}
/**
* Gets z coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFz() {
return estimatedBiases != null ? estimatedBiases[2] : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFxAsAcceleration() {
return estimatedBiases != null
? new Acceleration(estimatedBiases[0], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets x coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFxAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[0]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @return y coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFyAsAcceleration() {
return estimatedBiases != null
? new Acceleration(estimatedBiases[1], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets y coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFyAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[1]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @return z coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Acceleration getEstimatedBiasFzAsAcceleration() {
return estimatedBiases != null
? new Acceleration(estimatedBiases[2], AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets z coordinate of estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if result was updated, false if estimation is not available.
*/
@Override
public boolean getEstimatedBiasFzAsAcceleration(final Acceleration result) {
if (estimatedBiases != null) {
result.setValue(estimatedBiases[2]);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer bias.
*
* @return estimated accelerometer bias or null if not available.
*/
@Override
public AccelerationTriad getEstimatedBiasAsTriad() {
return estimatedBiases != null
? new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2])
: null;
}
/**
* Gets estimated accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated accelerometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedBiasAsTriad(final AccelerationTriad result) {
if (estimatedBiases != null) {
result.setValueCoordinatesAndUnit(
estimatedBiases[0], estimatedBiases[1], estimatedBiases[2],
AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets estimated accelerometer scale factors and cross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 639 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1658 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2379 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2512 |
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @return array containing x,y,z components of estimated magnetometer
* hard-iron biases.
*/
@Override
public double[] getEstimatedHardIron() {
return estimatedHardIron;
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @param result instance where estimated magnetometer biases will be
* stored.
* @return true if result instance was updated, false otherwise (when
* estimation is not yet available).
*/
@Override
public boolean getEstimatedHardIron(final double[] result) {
if (estimatedHardIron != null) {
System.arraycopy(estimatedHardIron, 0, result, 0, estimatedHardIron.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @return column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases.
*/
@Override
public Matrix getEstimatedHardIronAsMatrix() {
return estimatedHardIron != null ? Matrix.newFromArray(estimatedHardIron) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated
* magnetometer hard-iron biases expressed in Teslas (T).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedHardIronAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedHardIron != null) {
result.fromArray(estimatedHardIron);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return x coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronX() {
return estimatedHardIron != null ? estimatedHardIron[0] : null;
}
/**
* Gets y coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return y coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronY() {
return estimatedHardIron != null ? estimatedHardIron[1] : null;
}
/**
* Gets z coordinate of estimated magnetometer bias expressed in
* Teslas (T).
*
* @return z coordinate of estimated magnetometer bias or null if not
* available.
*/
@Override
public Double getEstimatedHardIronZ() {
return estimatedHardIron != null ? estimatedHardIron[2] : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @return x coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronXAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[0], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets x coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[0]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @return y coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronYAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[1], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets y coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[1]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @return z coordinate of estimated magnetometer bias.
*/
@Override
public MagneticFluxDensity getEstimatedHardIronZAsMagneticFluxDensity() {
return estimatedHardIron != null
? new MagneticFluxDensity(estimatedHardIron[2], MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets z coordinate of estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available, false otherwise.
*/
@Override
public boolean getEstimatedHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedHardIron != null) {
result.setValue(estimatedHardIron[2]);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer bias.
*
* @return estimated magnetometer bias or null if not available.
*/
@Override
public MagneticFluxDensityTriad getEstimatedHardIronAsTriad() {
return estimatedHardIron != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2]) : null;
}
/**
* Gets estimated magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if estimated magnetometer bias is available and result was
* modified, false otherwise.
*/
@Override
public boolean getEstimatedHardIronAsTriad(final MagneticFluxDensityTriad result) {
if (estimatedHardIron != null) {
result.setValueCoordinatesAndUnit(
estimatedHardIron[0], estimatedHardIron[1], estimatedHardIron[2], MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1924 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 1952 |
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets position where body magnetic flux density measurements have been
* taken.
*
* @return position where body magnetic flux density measurements have
* been taken.
*/
public NEDPosition getNedPosition() {
return position;
}
/**
* Sets position where body magnetic flux density measurements have been
* taken.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final NEDPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = position;
}
/**
* Gets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @return position where body magnetic flux density measurements have
* been taken or null if not available.
*/
public ECEFPosition getEcefPosition() {
if (position != null) {
final var result = new ECEFPosition();
getEcefPosition(result);
return result;
} else {
return null;
}
}
/**
* Gets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @param result instance where result will be stored.
* @return true if ECEF position could be computed, false otherwise.
*/
public boolean getEcefPosition(final ECEFPosition result) {
if (position != null) {
final var velocity = new ECEFVelocity();
NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
position.getLatitude(), position.getLongitude(), position.getHeight(),
0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @param position position where body magnetic flux density have been
* taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final ECEFPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = convertPosition(position);
}
/**
* Gets timestamp expressed as decimal year where magnetic flux density
* measurements have been measured.
*
* @return timestamp expressed as decimal year or null if not defined.
*/
public Double getYear() {
return year;
}
/**
* Sets timestamp expressed as decimal year where magnetic flux density
* measurements have been measured.
*
* @param year timestamp expressed as decimal year.
* @throws LockedException if calibrator is currently running.
*/
public void setYear(final Double year) throws LockedException {
if (running) {
throw new LockedException();
}
this.year = year;
}
/**
* Sets timestamp when magnetic flux density measurements have been
* measured.
*
* @param timestampMillis a timestamp expressed in milliseconds since
* epoch time (January 1st, 1970 at midnight).
* @throws LockedException if calibrator is currently running.
*/
public void setTime(final Long timestampMillis) throws LockedException {
if (running) {
throw new LockedException();
}
year = convertTime(timestampMillis);
}
/**
* Sets timestamp when magnetic flux density measurements have been
* measured.
*
* @param date a date instance containing a timestamp.
* @throws LockedException if calibrator is currently running.
*/
public void setTime(final Date date) throws LockedException {
if (running) {
throw new LockedException();
}
year = convertTime(date);
}
/**
* Sets timestamp when magnetic flux density measurements have been
* measured.
*
* @param calendar a calendar instance containing a timestamp.
* @throws LockedException if calibrator is currently running.
*/
public void setTime(final GregorianCalendar calendar) throws LockedException {
if (running) {
throw new LockedException();
}
year = convertTime(calendar);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public List<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
return measurements;
}
/**
* Sets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @param measurements collection of body magnetic flux density
* measurements at a known position and timestamp
* with unknown orientations.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final List<StandardDeviationBodyMagneticFluxDensity> measurements)
throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public MagnetometerCalibratorMeasurementType getMeasurementType() {
return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer.
* When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
*
* @return true if z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer, false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mm matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for
* accelerometer, gyroscope and magnetometer, false
* otherwise.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this calibrator.
*
* @return listener to handle events raised by this calibrator.
*/
public RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3380 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4642 |
}
/**
* Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated x coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasXStandardDeviation() {
final var variance = getEstimatedBiasXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasXStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated y coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasYStandardDeviation() {
final var variance = getEstimatedBiasYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasYStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated z coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasZStandardDeviation() {
final var variance = getEstimatedBiasZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasZStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @return standard deviation of estimated gyroscope bias coordinates.
*/
public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of gyroscope bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation(),
AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates expressed
* in radians per second (rad/s).
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
+ getEstimatedBiasZStandardDeviation()) / 3.0
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null.
*/
public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of gyroscope bias expressed in
* radians per second (rad/s).
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias coordinates.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of gyroscope bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences()}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than
* {@link #getMinimumRequiredMeasurementsOrSequences}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < getMinimumRequiredMeasurementsOrSequences()) {
throw new IllegalArgumentException();
}
this.preliminarySubsetSize = preliminarySubsetSize;
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
public abstract RobustEstimatorMethod getMethod();
/**
* Creates a robust gyroscope calibrator.
*
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
*/
public static RobustEasyGyroscopeCalibrator create(final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3556 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3567 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @return constant rotation rate of turntable.
*/
public double getTurntableRotationRate() {
return turntableRotationRate;
}
/**
* Sets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(final double turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
if (turntableRotationRate <= 0.0) {
throw new IllegalArgumentException();
}
this.turntableRotationRate = turntableRotationRate;
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @return constant rotation rate of turntable.
*/
public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @param result instance where result will be stored.
*/
public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
result.setValue(turntableRotationRate);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets constant rotation rate at which the turntable is spinning.
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
}
/**
* Gets time interval between measurements being captured expressed in
* seconds (s).
*
* @return time interval between measurements.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between measurements being captured expressed in
* seconds (s).
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval <= 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between measurements being captured.
*
* @return time interval between measurements.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between measurements being captured.
*
* @param result instance where result will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between measurements being captured.
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
setTimeInterval(convertTime(timeInterval));
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3515 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3603 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @return constant rotation rate of turntable.
*/
public double getTurntableRotationRate() {
return turntableRotationRate;
}
/**
* Sets constant rotation rate at which the turntable is spinning.
* This is expressed in radians per second (rad/s).
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(
final double turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
if (turntableRotationRate <= 0.0) {
throw new IllegalArgumentException();
}
this.turntableRotationRate = turntableRotationRate;
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @return constant rotation rate of turntable.
*/
public AngularSpeed getTurntableRotationRateAsAngularSpeed() {
return new AngularSpeed(turntableRotationRate, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets constant rotation rate at which the turntable is spinning.
*
* @param result instance where result will be stored.
*/
public void getTurntableRotationRateAsAngularSpeed(final AngularSpeed result) {
result.setValue(turntableRotationRate);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets constant rotation rate at which the turntable is spinning.
*
* @param turntableRotationRate constant rotation rate of turntable.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTurntableRotationRate(final AngularSpeed turntableRotationRate) throws LockedException {
if (running) {
throw new LockedException();
}
setTurntableRotationRate(convertAngularSpeed(turntableRotationRate));
}
/**
* Gets time interval between measurements being captured expressed in
* seconds (s).
*
* @return time interval between measurements.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between measurements being captured expressed in
* seconds (s).
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is zero or
* negative.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval <= 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between measurements being captured.
*
* @return time interval between measurements.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between measurements being captured.
*
* @param result instance where result will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between measurements being captured.
*
* @param timeInterval time interval between measurements.
* @throws LockedException if calibrator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
setTimeInterval(convertTime(timeInterval));
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public List<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3380 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 2086 |
}
/**
* Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated x coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasXStandardDeviation() {
final var variance = getEstimatedBiasXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasXStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated y coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasYStandardDeviation() {
final var variance = getEstimatedBiasYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasYStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated z coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasZStandardDeviation() {
final var variance = getEstimatedBiasZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasZStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @return standard deviation of estimated gyroscope bias coordinates.
*/
public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of gyroscope bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation(),
AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates expressed
* in radians per second (rad/s).
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
+ getEstimatedBiasZStandardDeviation()) / 3.0
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null.
*/
public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of gyroscope bias expressed in
* radians per second (rad/s).
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias coordinates.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of gyroscope bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences()}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than
* {@link #getMinimumRequiredMeasurementsOrSequences}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < getMinimumRequiredMeasurementsOrSequences()) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3257 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3610 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3368 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4549 |
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated x coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasXStandardDeviation() {
final var variance = getEstimatedBiasXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasXStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated y coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasYStandardDeviation() {
final var variance = getEstimatedBiasYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasYStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated z coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasZStandardDeviation() {
final var variance = getEstimatedBiasZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasZStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @return standard deviation of estimated gyroscope bias coordinates.
*/
public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of gyroscope bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation(),
AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates expressed
* in radians per second (rad/s).
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
+ getEstimatedBiasZStandardDeviation()) / 3.0 : null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null.
*/
public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of gyroscope bias expressed in
* radians per second (rad/s).
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias coordinates.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of gyroscope bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java | 901 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java | 903 |
this(convertPosition(position), measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Gets position where body magnetic flux density measurements have been
* taken.
*
* @return position where body magnetic flux density measurements have
* been taken.
*/
public NEDPosition getNedPosition() {
return position;
}
/**
* Sets position where body magnetic flux density measurements have been
* taken.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final NEDPosition position) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
this.position = position;
}
/**
* Gets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @return position where body magnetic flux density measurements have
* been taken or null if not available.
*/
public ECEFPosition getEcefPosition() {
if (position != null) {
final var result = new ECEFPosition();
getEcefPosition(result);
return result;
} else {
return null;
}
}
/**
* Gets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @param result instance where result will be stored.
* @return true if ECEF position could be computed, false otherwise.
*/
public boolean getEcefPosition(final ECEFPosition result) {
if (position != null) {
final var velocity = new ECEFVelocity();
NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
position.getLatitude(), position.getLongitude(), position.getHeight(),
0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @param position position where body magnetic flux density have been
* taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final ECEFPosition position) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
this.position = convertPosition(position);
}
/**
* Gets timestamp expressed as decimal year where magnetic flux density
* measurements have been measured.
*
* @return timestamp expressed as decimal year or null if not defined.
*/
public Double getYear() {
return year;
}
/**
* Sets timestamp expressed as decimal year where magnetic flux density
* measurements have been measured.
*
* @param year timestamp expressed as decimal year.
* @throws LockedException if calibrator is currently running.
*/
public void setYear(final Double year) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
this.year = year;
}
/**
* Sets timestamp when magnetic flux density measurements have been
* measured.
*
* @param timestampMillis a timestamp expressed in milliseconds since
* epoch time (January 1st, 1970 at midnight).
* @throws LockedException if calibrator is currently running.
*/
public void setTime(final Long timestampMillis) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
year = convertTime(timestampMillis);
}
/**
* Sets timestamp when magnetic flux density measurements have been
* measured.
*
* @param date a date instance containing a timestamp.
* @throws LockedException if calibrator is currently running.
*/
public void setTime(final Date date) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
year = convertTime(date);
}
/**
* Sets timestamp when magnetic flux density measurements have been
* measured.
*
* @param calendar a calendar instance containing a timestamp.
* @throws LockedException if calibrator is currently running.
*/
public void setTime(final GregorianCalendar calendar) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
year = convertTime(calendar);
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && position != null && year != null;
}
/**
* Gets Earth's magnetic model.
*
* @return Earth's magnetic model or null if not provided.
*/
public WorldMagneticModel getMagneticModel() {
return magneticModel;
}
/**
* Sets Earth's magnetic model.
*
* @param magneticModel Earth's magnetic model to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
this.magneticModel = magneticModel;
}
/**
* Called before calibration occurs.
* This can be overridden by subclasses.
*
* @throws CalibrationException if anything fails.
*/
@Override
protected void onBeforeCalibrate() throws CalibrationException {
computeGroundTruthMagneticFluxDensityNorm();
}
/**
* Computes expected ground truth magnetic flux density norm based on World Magnetic Model.
* Notice that actual magnetic field measured by devices might differ from this value, since it might be attenuated
* or other devices might interfere.
*
* @throws CalibrationException if world magnetic model cannot be loaded.
*/
private void computeGroundTruthMagneticFluxDensityNorm() throws CalibrationException {
final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
if (magneticModel != null) {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
} else {
try {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
} catch (final IOException e) {
throw new CalibrationException(e);
}
}
final var pos = getNedPosition();
final var earthB = wmmEstimator.estimate(pos, year);
groundTruthMagneticFluxDensityNorm = earthB.getNorm();
}
/**
* Converts a time instance expressed in milliseconds since epoch time
* (January 1st, 1970 at midnight) to a decimal year.
*
* @param timestampMillis milliseconds value to be converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final Long timestampMillis) {
if (timestampMillis == null) {
return null;
}
final var calendar = new GregorianCalendar();
calendar.setTimeInMillis(timestampMillis);
return convertTime(calendar);
}
/**
* Converts a time instant contained ina date object to a
* decimal year.
*
* @param date a time instance to be converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final Date date) {
if (date == null) {
return null;
}
final var calendar = new GregorianCalendar();
calendar.setTime(date);
return convertTime(calendar);
}
/**
* Converts a time instant contained in a gregorian calendar to a
* decimal year.
*
* @param calendar calendar containing a specific instant to be
* converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final GregorianCalendar calendar) {
if (calendar == null) {
return null;
}
return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
}
/**
* Converts provided ECEF position to position expressed in NED
* coordinates.
*
* @param position ECEF position to be converted.
* @return converted position expressed in NED coordinates.
*/
private static NEDPosition convertPosition(final ECEFPosition position) {
final var velocity = new NEDVelocity();
final var result = new NEDPosition();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
position.getX(), position.getY(), position.getZ(),
0.0, 0.0, 0.0, result, velocity);
return result;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4409 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4743 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @param measurements collection of body kinematics measurements at a
* known position with unknown orientations.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final Collection<StandardDeviationBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public AccelerometerCalibratorMeasurementType getMeasurementType() {
return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public L getListener() {
return listener;
}
/**
* Sets listener to handle events raised by this estimator.
*
* @param listener listener to handle events raised by this estimator.
* @throws LockedException if calibrator is currently running.
*/
public void setListener(final L listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= getMinimumRequiredMeasurements()
&& groundTruthGravityNorm != null;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
//noinspection unchecked
listener.onCalibrateStart((C) this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
//noinspection unchecked
listener.onCalibrateEnd((C) this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1797 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1879 |
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
return measurements;
}
/**
* Sets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @param measurements collection of body magnetic flux density
* measurements at a known position and timestamp
* with unknown orientations.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final Collection<StandardDeviationBodyMagneticFluxDensity> measurements)
throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public MagnetometerCalibratorMeasurementType getMeasurementType() {
return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer.
* When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
*
* @return true if z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer, false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mm matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for
* accelerometer, gyroscope and magnetometer, false
* otherwise.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this calibrator.
*
* @return listener to handle events raised by this calibrator.
*/
public L getListener() {
return listener;
}
/**
* Sets listener to handle events raised by this calibrator.
*
* @param listener listener to handle events raised by this calibrator.
* @throws LockedException if calibrator is currently running.
*/
public void setListener(final L listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= getMinimumRequiredMeasurements();
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates magnetometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
onBeforeCalibrate();
if (listener != null) {
//noinspection unchecked
listener.onCalibrateStart((C) this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
//noinspection unchecked
listener.onCalibrateEnd((C) this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2064 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2455 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2061 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 719 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3125 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3164 |
public void setInitialBias(
final AngularSpeed initialBiasX, final AngularSpeed initialBiasY, final AngularSpeed initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAngularSpeed(initialBiasX);
this.initialBiasY = convertAngularSpeed(initialBiasY);
this.initialBiasZ = convertAngularSpeed(initialBiasZ);
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3902 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4606 |
if (estimatedMg == null) {
estimatedMg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedMg.initialize(0.0);
}
estimatedMg.setElementAt(0, 0, sx);
estimatedMg.setElementAt(1, 0, myx);
estimatedMg.setElementAt(2, 0, mzx);
estimatedMg.setElementAt(0, 1, mxy);
estimatedMg.setElementAt(1, 1, sy);
estimatedMg.setElementAt(2, 1, mzy);
estimatedMg.setElementAt(0, 2, mxz);
estimatedMg.setElementAt(1, 2, myz);
estimatedMg.setElementAt(2, 2, sz);
if (estimatedGg == null) {
estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedGg.initialize(0.0);
}
estimatedGg.setElementAtIndex(0, g11);
estimatedGg.setElementAtIndex(1, g21);
estimatedGg.setElementAtIndex(2, g31);
estimatedGg.setElementAtIndex(3, g12);
estimatedGg.setElementAtIndex(4, g22);
estimatedGg.setElementAtIndex(5, g32);
estimatedGg.setElementAtIndex(6, g13);
estimatedGg.setElementAtIndex(7, g23);
estimatedGg.setElementAtIndex(8, g33);
estimatedCovariance = fitter.getCovar();
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Sets input data into Levenberg-Marquardt fitter.
*
* @throws WrongSizeException never happens.
*/
private void setInputData() throws WrongSizeException {
// set input data using:
// Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez
final var expectedKinematics = new BodyKinematics();
final var numMeasurements = measurements.size();
final var x = new Matrix(numMeasurements, 2 * BodyKinematics.COMPONENTS);
final var y = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
final var standardDeviations = new double[numMeasurements];
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var omegaMeasX = measuredKinematics.getAngularRateX();
final var omegaMeasY = measuredKinematics.getAngularRateY();
final var omegaMeasZ = measuredKinematics.getAngularRateZ();
final var omegaTrueX = expectedKinematics.getAngularRateX();
final var omegaTrueY = expectedKinematics.getAngularRateY();
final var omegaTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
x.setElementAt(i, 0, omegaTrueX);
x.setElementAt(i, 1, omegaTrueY);
x.setElementAt(i, 2, omegaTrueZ);
x.setElementAt(i, 3, fTrueX);
x.setElementAt(i, 4, fTrueY);
x.setElementAt(i, 5, fTrueZ);
y.setElementAt(i, 0, omegaMeasX);
y.setElementAt(i, 1, omegaMeasY);
y.setElementAt(i, 2, omegaMeasZ);
standardDeviations[i] = measurement.getAngularRateStandardDeviation();
i++;
}
fitter.setInputData(x, y, standardDeviations);
}
/**
* Converts angular speed instance to radians per second (rad/s).
*
* @param value angular speed value.
* @param unit unit of angular speed value.
* @return converted value.
*/
private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Converts angular speed instance to radians per second (rad/s).
*
* @param angularSpeed angular speed instance to be converted.
* @return converted value.
*/
private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2766 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4002 |
return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences()}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #getMinimumRequiredMeasurementsOrSequences}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than
* {@link #getMinimumRequiredMeasurementsOrSequences}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < getMinimumRequiredMeasurementsOrSequences()) {
throw new IllegalArgumentException();
}
this.preliminarySubsetSize = preliminarySubsetSize;
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
public abstract RobustEstimatorMethod getMethod();
/**
* Creates a robust gyroscope calibrator.
*
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5384 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3360 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 2011 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2669 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2628 |
}
/**
* Gets variance of estimated x coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated x coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFxVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated x coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFxStandardDeviation() {
final var variance = getEstimatedBiasFxVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias.
*
* @return standard deviation of estimated x coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFxStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFxStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFxStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFxStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated y coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFyVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated y coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFyStandardDeviation() {
final var variance = getEstimatedBiasFyVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias.
*
* @return standard deviation of estimated y coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFyStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFyStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFyStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFyStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated z coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFzVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias expressed in
* meters per squared second (m/s^2).
*
* @return standard deviation of estimated z coordinate of accelerometer bias or null if not
* available.
*/
public Double getEstimatedBiasFzStandardDeviation() {
final var variance = getEstimatedBiasFzVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias.
*
* @return standard deviation of estimated z coordinate of accelerometer bias or null if not
* available.
*/
public Acceleration getEstimatedBiasFzStandardDeviationAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasFzStandardDeviation(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of accelerometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasFzStandardDeviationAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasFzStandardDeviation());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated accelerometer bias coordinates.
*
* @return standard deviation of estimated accelerometer bias coordinates.
*/
public AccelerationTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null ?
new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
getEstimatedBiasFxStandardDeviation(),
getEstimatedBiasFyStandardDeviation(),
getEstimatedBiasFzStandardDeviation()) : null;
}
/**
* Gets standard deviation of estimated accelerometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of accelerometer bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AccelerationTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasFxStandardDeviation(),
getEstimatedBiasFyStandardDeviation(),
getEstimatedBiasFzStandardDeviation(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates expressed
* in meters per squared second (m/s^2).
*
* @return average of estimated standard deviation of accelerometer bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null ?
(getEstimatedBiasFxStandardDeviation() + getEstimatedBiasFyStandardDeviation()
+ getEstimatedBiasFzStandardDeviation()) / 3.0 : null;
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates.
*
* @return average of estimated standard deviation of accelerometer bias coordinates or null.
*/
public Acceleration getEstimatedBiasStandardDeviationAverageAsAcceleration() {
return estimatedCovariance != null ?
new Acceleration(getEstimatedBiasStandardDeviationAverage(),
AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets average of estimated standard deviation of accelerometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of accelerometer bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of accelerometer bias expressed in
* meters per squared second (m/s^2).
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of accelerometer bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasFxVariance() + getEstimatedBiasFyVariance() + getEstimatedBiasFzVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of accelerometer bias.
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of accelerometer bias or null
* if not available.
*/
public Acceleration getEstimatedBiasStandardDeviationNormAsAcceleration() {
return estimatedCovariance != null
? new Acceleration(getEstimatedBiasStandardDeviationNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of accelerometer bias coordinates.
* This can be used as the initial accelerometer bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of accelerometer bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAcceleration(final Acceleration result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3269 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3622 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 2086 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4642 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4561 |
}
/**
* Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated x coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasXStandardDeviation() {
final var variance = getEstimatedBiasXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @return standard deviation of estimated x coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasXStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasXStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasXStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasXStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated y coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasYStandardDeviation() {
final var variance = getEstimatedBiasYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @return standard deviation of estimated y coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasYStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasYStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasYStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasYStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated z coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias expressed in
* radians per second (rad/s).
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public Double getEstimatedBiasZStandardDeviation() {
final var variance = getEstimatedBiasZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @return standard deviation of estimated z coordinate of gyroscope bias or null if not
* available.
*/
public AngularSpeed getEstimatedBiasZStandardDeviationAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasZStandardDeviation(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of gyroscope bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasZStandardDeviationAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasZStandardDeviation());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @return standard deviation of estimated gyroscope bias coordinates.
*/
public AngularSpeedTriad getEstimatedBiasStandardDeviation() {
return estimatedCovariance != null
? new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND,
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of gyroscope bias was available, false
* otherwise.
*/
public boolean getEstimatedBiasStandardDeviation(final AngularSpeedTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedBiasXStandardDeviation(),
getEstimatedBiasYStandardDeviation(),
getEstimatedBiasZStandardDeviation(),
AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates expressed
* in radians per second (rad/s).
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null
* if not available.
*/
public Double getEstimatedBiasStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedBiasXStandardDeviation() + getEstimatedBiasYStandardDeviation()
+ getEstimatedBiasZStandardDeviation()) / 3.0 : null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @return average of estimated standard deviation of gyroscope bias coordinates or null.
*/
public AngularSpeed getEstimatedBiasStandardDeviationAverageAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationAverage(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets average of estimated standard deviation of gyroscope bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of gyroscope bias is available,
* false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationAverageAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationAverage());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of gyroscope bias expressed in
* radians per second (rad/s).
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
@Override
public Double getEstimatedBiasStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedBiasXVariance() + getEstimatedBiasYVariance() + getEstimatedBiasZVariance())
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @return norm of estimated standard deviation of gyroscope bias or null
* if not available.
*/
public AngularSpeed getEstimatedBiasStandardDeviationNormAsAngularSpeed() {
return estimatedCovariance != null
? new AngularSpeed(getEstimatedBiasStandardDeviationNorm(), AngularSpeedUnit.RADIANS_PER_SECOND)
: null;
}
/**
* Gets norm of estimated standard deviation of gyroscope bias coordinates.
* This can be used as the initial gyroscope bias uncertainty for
* {@link INSLooselyCoupledKalmanInitializerConfig} or {@link INSTightlyCoupledKalmanInitializerConfig}.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of gyroscope bias is
* available, false otherwise.
*/
public boolean getEstimatedBiasStandardDeviationNormAsAngularSpeed(final AngularSpeed result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedBiasStandardDeviationNorm());
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
return true;
} else {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2529 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4105 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 2040 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2762 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2895 |
}
/**
* Gets variance of estimated x coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated x coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronXVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(0, 0) : null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated x coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronXStandardDeviation() {
final var variance = getEstimatedHardIronXVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias.
*
* @return standard deviation of estimated x coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronXStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated x coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated x coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronXStandardDeviationAsMagneticFluxDensity(
final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronXStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated y coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated y coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronYVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(1, 1) : null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated y coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronYStandardDeviation() {
final var variance = getEstimatedHardIronYVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias.
*
* @return standard deviation of estimated y coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronYStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated y coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated y coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronYStandardDeviationAsMagneticFluxDensity(
final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronYStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets variance of estimated z coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated z coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronZVariance() {
return estimatedCovariance != null ? estimatedCovariance.getElementAt(2, 2) : null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias
* expressed in Teslas (T).
*
* @return standard deviation of estimated z coordinate of magnetometer bias
* or null if not available.
*/
public Double getEstimatedHardIronZStandardDeviation() {
final var variance = getEstimatedHardIronZVariance();
return variance != null ? Math.sqrt(variance) : null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias.
*
* @return standard deviation of estimated z coordinate of magnetometer bias
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronZStandardDeviation(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets standard deviation of estimated z coordinate of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if standard deviation of estimated z coordinate of
* magnetometer bias is available, false otherwise.
*/
public boolean getEstimatedHardIronZStandardDeviationAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronZStandardDeviation());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets standard deviation of estimated magnetometer bias coordinates.
*
* @return standard deviation of estimated magnetometer bias coordinates.
*/
public MagneticFluxDensityTriad getEstimatedHardIronStandardDeviation() {
return estimatedCovariance != null
? new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA,
getEstimatedHardIronXStandardDeviation(),
getEstimatedHardIronYStandardDeviation(),
getEstimatedHardIronZStandardDeviation())
: null;
}
/**
* Gets standard deviation of estimated magnetometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if standard deviation of magnetometer bias was available,
* false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviation(final MagneticFluxDensityTriad result) {
if (estimatedCovariance != null) {
result.setValueCoordinatesAndUnit(
getEstimatedHardIronXStandardDeviation(),
getEstimatedHardIronYStandardDeviation(),
getEstimatedHardIronZStandardDeviation(),
MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates
* expressed in Teslas (T).
*
* @return average of estimated standard deviation of magnetometer bias coordinates,
* or null if not available.
*/
public Double getEstimatedHardIronStandardDeviationAverage() {
return estimatedCovariance != null
? (getEstimatedHardIronXStandardDeviation() + getEstimatedHardIronYStandardDeviation()
+ getEstimatedHardIronZStandardDeviation()) / 3.0
: null;
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates.
*
* @return average of estimated standard deviation of magnetometer bias coordinates,
* or null if not available.
*/
public MagneticFluxDensity getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationAverage(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets average of estimated standard deviation of magnetometer bias coordinates.
*
* @param result instance where result will be stored.
* @return true if average of estimated standard deviation of magnetometer bias is available,
* false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviationAverageAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronStandardDeviationAverage());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Gets norm of estimated standard deviation of magnetometer bias expressed in
* Teslas (T).
*
* @return norm of estimated standard deviation of magnetometer bias or null
* if not available.
*/
public Double getEstimatedHardIronStandardDeviationNorm() {
return estimatedCovariance != null
? Math.sqrt(getEstimatedHardIronXVariance() + getEstimatedHardIronYVariance()
+ getEstimatedHardIronZVariance()) : null;
}
/**
* Gets norm of estimated standard deviation of magnetometer bias.
*
* @return norm of estimated standard deviation of magnetometer bias or null
* if not available.
*/
public MagneticFluxDensity getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity() {
return estimatedCovariance != null
? new MagneticFluxDensity(getEstimatedHardIronStandardDeviationNorm(), MagneticFluxDensityUnit.TESLA)
: null;
}
/**
* Gets norm of estimated standard deviation of magnetometer bias.
*
* @param result instance where result will be stored.
* @return true if norm of estimated standard deviation of magnetometer bias
* is available, false otherwise.
*/
public boolean getEstimatedHardIronStandardDeviationNormAsMagneticFluxDensity(
final MagneticFluxDensity result) {
if (estimatedCovariance != null) {
result.setValue(getEstimatedHardIronStandardDeviationNorm());
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2055 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2013 |
&& groundTruthGravityNorm != null;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration solution.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
* This is only available when result has been refined and covariance
* is kept.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #getMinimumRequiredMeasurements()}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #getMinimumRequiredMeasurements()}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than {@link #getMinimumRequiredMeasurements()}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < getMinimumRequiredMeasurements()) {
throw new IllegalArgumentException();
}
this.preliminarySubsetSize = preliminarySubsetSize;
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
public abstract RobustEstimatorMethod getMethod();
/**
* Creates a robust accelerometer calibrator.
*
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 1823 |
this.listener = listener;
}
/**
* Gets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return x coordinate of accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasX x coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return y coordinate of accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasY y coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return z coordinate of accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasZ z coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x coordinate of accelerometer bias.
*
* @return x coordinate of accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() {
return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x coordinate of accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasXAsAcceleration(final Acceleration result) {
result.setValue(biasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x coordinate of accelerometer bias.
*
* @param biasX x coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasX(final Acceleration biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
}
/**
* Gets known y coordinate of accelerometer bias.
*
* @return y coordinate of accelerometer bias.
*/
@Override
public Acceleration getBiasYAsAcceleration() {
return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y coordinate of accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasYAsAcceleration(final Acceleration result) {
result.setValue(biasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y coordinate of accelerometer bias.
*
* @param biasY y coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasY(final Acceleration biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAcceleration(biasY);
}
/**
* Gets known z coordinate of accelerometer bias.
*
* @return z coordinate of accelerometer bias.
*/
@Override
public Acceleration getBiasZAsAcceleration() {
return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z coordinate of accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasZAsAcceleration(final Acceleration result) {
result.setValue(biasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z coordinate of accelerometer bias.
*
* @param biasZ z coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasZ(final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAcceleration(biasZ);
}
/**
* Sets known accelerometer bias coordinates expressed in meters per squared
* second (m/s^2).
*
* @param biasX x coordinate of accelerometer bias.
* @param biasY y coordinate of accelerometer bias.
* @param biasZ z coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known accelerometer bias coordinates.
*
* @param biasX z coordinate of accelerometer bias.
* @param biasY y coordinate of accelerometer bias.
* @param biasZ z coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasCoordinates(final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
this.biasY = convertAcceleration(biasY);
this.biasZ = convertAcceleration(biasZ);
}
/**
* Gets known accelerometer bias.
*
* @return known accelerometer bias.
*/
@Override
public AccelerationTriad getBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known accelerometer bias.
*
* @param result instance where result will be stored.
*/
@Override
public void getBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known accelerometer bias.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBias(final AccelerationTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3714 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5033 |
setResult(m, b, g);
// at this point covariance is expressed in terms of b, M and G, and must
// be expressed in terms of bg, Mg and Gg.
// We know that:
// bg = M * b
// Mg = M - I
// Gg = M * G
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [0 m22 m23]
// [0 0 m33]
// G = [g11 g12 g13]
// [g21 g22 g23]
// [g31 g32 g33]
// bg = [m11 m12 m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
// [0 m22 m23][by] [ m22 * by + m23 * bz] [bgy]
// [0 0 m33][bz] [ m33 * bz] [bgz]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [0 m22 - 1 m23 ]
// [mzx mzy sz ] [0 0 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = [m11 m12 m13][g11 g12 g13]
// [gg21 gg22 gg23] [0 m22 m23][g21 g22 g23]
// [gg31 gg32 gg33] [0 0 m33][g31 g32 g33]
// Defining the linear application:
// F(b, M, G) = F(bx, by, bz, m11, m12, m22, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
// as:
// [bgx] = [m11 * bx + m12 * by + m13 * bz]
// [bgy] [ m22 * by + m23 * bz]
// [bgz] [ m33 * bz]
// [sx] [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [0]
// [myz] [m23]
// [mzx] [0]
// [mzy] [0]
// [gg11] [m11 * g11 + m12 * g21 + m13 * g31]
// [gg21] [ m22 * g21 + m23 * g31]
// [gg31] [ m33 * g31]
// [gg12] [m11 * g12 + m12 * g22 + m13 * g32]
// [gg22] [ m22 * g22 + m23 * g32]
// [gg32] [ m33 * g32]
// [gg13] [m11 * g13 + m12 * g23 + m13 * g33]
// [gg23] [ m22 * g23 + m23 * g33]
// [gg33] [ m33 * g33]
// Then the Jacobian of F(b, M, G) is:
// J = [m11 m12 m13 bx by 0 bz 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 m22 m23 0 0 by 0 bz 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 m33 0 0 0 0 0 bz 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 g11 g21 0 g31 0 0 m11 m12 m13 0 0 0 0 0 0 ]
// [0 0 0 0 0 g21 0 g31 0 0 m22 m23 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 g31 0 0 m33 0 0 0 0 0 0 ]
// [0 0 0 g12 g22 0 g32 0 0 0 0 0 m11 m12 m13 0 0 0 ]
// [0 0 0 0 0 g22 0 g32 0 0 0 0 0 m22 m23 0 0 0 ]
// [0 0 0 0 0 0 0 0 g32 0 0 0 0 0 m33 0 0 0 ]
// [0 0 0 g13 g23 0 g33 0 0 0 0 0 0 0 0 m11 m12 m13]
// [0 0 0 0 0 g23 0 g33 0 0 0 0 0 0 0 0 m22 m23]
// [0 0 0 0 0 0 0 0 g33 0 0 0 0 0 0 0 0 m33]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES);
jacobian.setElementAt(0, 0, m11);
jacobian.setElementAt(0, 1, m12);
jacobian.setElementAt(0, 2, m13);
jacobian.setElementAt(0, 3, bx);
jacobian.setElementAt(0, 4, by);
jacobian.setElementAt(0, 6, bz);
jacobian.setElementAt(1, 1, m22);
jacobian.setElementAt(1, 2, m23);
jacobian.setElementAt(1, 5, by);
jacobian.setElementAt(1, 7, bz);
jacobian.setElementAt(2, 2, m33);
jacobian.setElementAt(2, 8, bz);
jacobian.setElementAt(3, 3, 1.0);
jacobian.setElementAt(4, 5, 1.0);
jacobian.setElementAt(5, 8, 1.0);
jacobian.setElementAt(6, 4, 1.0);
jacobian.setElementAt(7, 6, 1.0);
jacobian.setElementAt(9, 7, 1.0);
jacobian.setElementAt(12, 3, g11);
jacobian.setElementAt(12, 4, g21);
jacobian.setElementAt(12, 6, g31);
jacobian.setElementAt(12, 9, m11);
jacobian.setElementAt(12, 10, m12);
jacobian.setElementAt(12, 11, m13);
jacobian.setElementAt(13, 5, g21);
jacobian.setElementAt(13, 7, g31);
jacobian.setElementAt(13, 10, m22);
jacobian.setElementAt(13, 11, m23);
jacobian.setElementAt(14, 8, g31);
jacobian.setElementAt(14, 11, m33);
jacobian.setElementAt(15, 3, g12);
jacobian.setElementAt(15, 4, g22);
jacobian.setElementAt(15, 6, g32);
jacobian.setElementAt(15, 12, m11);
jacobian.setElementAt(15, 13, m12);
jacobian.setElementAt(15, 14, m13);
jacobian.setElementAt(16, 5, g22);
jacobian.setElementAt(16, 7, g32);
jacobian.setElementAt(16, 13, m22);
jacobian.setElementAt(16, 14, m23);
jacobian.setElementAt(17, 8, g32);
jacobian.setElementAt(17, 14, m33);
jacobian.setElementAt(18, 3, g13);
jacobian.setElementAt(18, 4, g23);
jacobian.setElementAt(18, 6, g33);
jacobian.setElementAt(18, 15, m11);
jacobian.setElementAt(18, 16, m12);
jacobian.setElementAt(18, 17, m13);
jacobian.setElementAt(19, 5, g23);
jacobian.setElementAt(19, 7, g33);
jacobian.setElementAt(19, 16, m22);
jacobian.setElementAt(19, 17, m23);
jacobian.setElementAt(20, 8, g33);
jacobian.setElementAt(20, 17, m33);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Internal method to perform general calibration when G-dependent cross
* biases are being estimated.
*
* @throws AlgebraException if accelerometer parameters prevent fixing
* measured accelerometer values due to numerical instabilities.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateGeneralAndGDependentCrossBiases() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1328 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2722 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 480 |
this(measurements, commonAxisUsed, magneticModel, hardIron);
this.listener = listener;
}
/**
* Gets x-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return x-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronX() {
return hardIronX;
}
/**
* Sets x-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronX x coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final double hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
}
/**
* Gets y-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return y-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronY() {
return hardIronY;
}
/**
* Sets y-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronY y coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final double hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = hardIronY;
}
/**
* Gets z-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return z-coordinate of known magnetometer hard-iron bias.
*/
@Override
public double getHardIronZ() {
return hardIronZ;
}
/**
* Sets z-coordinate of known magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronZ z coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = hardIronZ;
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @return x coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known x-coordinate of magnetometer hard-iron.
*
* @param hardIronX known x-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @return y coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known y-coordinate of magnetometer hard-iron.
*
* @param hardIronY known y-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = convertMagneticFluxDensity(hardIronY);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @return z coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known z-coordinate of magnetometer hard-iron.
*
* @param hardIronZ known z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Sets known hard-iron bias coordinates of magnetometer expressed
* in Teslas (T).
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
this.hardIronY = hardIronY;
this.hardIronZ = hardIronZ;
}
/**
* Sets known hard-iron coordinates.
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
this.hardIronY = convertMagneticFluxDensity(hardIronY);
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Gets known hard-iron.
*
* @return known hard-iron.
*/
@Override
public MagneticFluxDensityTriad getHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
}
/**
* Gets known hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known hard-iron.
*
* @param hardIron hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
}
/**
* Gets a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<FrameBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 3758 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 709 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1118 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1074 |
}
/**
* Gets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() {
return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasXAsAcceleration(final Acceleration result) {
result.setValue(biasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets x-coordinate of known accelerometer bias.
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final Acceleration biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
}
/**
* Gets y-coordinate of known accelerometer bias.
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasYAsAcceleration() {
return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets y-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasYAsAcceleration(final Acceleration result) {
result.setValue(biasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets y-coordinate of known accelerometer bias.
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final Acceleration biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAcceleration(biasY);
}
/**
* Gets z-coordinate of known accelerometer bias.
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasZAsAcceleration() {
return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets z-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasZAsAcceleration(final Acceleration result) {
result.setValue(biasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets z-coordinate of known accelerometer bias to be used to find a solution.
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAcceleration(biasZ);
}
/**
* Sets known bias coordinates of accelerometer expressed in meters
* per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @param biasY y-coordinate of known accelerometer bias.
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known bias coordinates of accelerometer.
*
* @param biasX x-coordinate of known accelerometer bias.
* @param biasY y-coordinate of known accelerometer bias.
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(
final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
this.biasY = convertAcceleration(biasY);
this.biasZ = convertAcceleration(biasZ);
}
/**
* Gets known accelerometer bias.
*
* @return known accelerometer bias.
*/
@Override
public AccelerationTriad getBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known accelerometer bias.
*
* @param result instance where result will be stored.
*/
@Override
public void getBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known accelerometer bias.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBias(final AccelerationTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2171 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2305 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS_COMMON_Z_AXIS}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
throw new IllegalArgumentException();
}
this.preliminarySubsetSize = preliminarySubsetSize;
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
public abstract RobustEstimatorMethod getMethod();
/**
* Creates a robust magnetometer calibrator.
*
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1137 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1330 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1299 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1264 |
}
/**
* Gets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronX() {
return hardIronX;
}
/**
* Sets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronX known x-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final double hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
}
/**
* Gets known y-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known y-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronY() {
return hardIronY;
}
/**
* Sets known y-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @param hardIronY known y-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final double hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = hardIronY;
}
/**
* Gets known z-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known z-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronZ() {
return hardIronZ;
}
/**
* Sets known z-coordinate of magnetometer hard-iron bias.
* This is expressed in meters Teslas (T).
*
* @param hardIronZ known z-coordinate of magnetometer
* hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = hardIronZ;
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @return x coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronXAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronX, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known x coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronXAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronX);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known x-coordinate of magnetometer hard-iron.
*
* @param hardIronX known x-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronX(final MagneticFluxDensity hardIronX) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @return y coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronYAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronY, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known y coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronYAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronY);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known y-coordinate of magnetometer hard-iron.
*
* @param hardIronY known y-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronY(final MagneticFluxDensity hardIronY) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronY = convertMagneticFluxDensity(hardIronY);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @return z coordinate of magnetometer hard-iron.
*/
@Override
public MagneticFluxDensity getHardIronZAsMagneticFluxDensity() {
return new MagneticFluxDensity(hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Gets known z coordinate of magnetometer hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronZAsMagneticFluxDensity(final MagneticFluxDensity result) {
result.setValue(hardIronZ);
result.setUnit(MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known z-coordinate of magnetometer hard-iron.
*
* @param hardIronZ known z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronZ(final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Sets known hard-iron bias coordinates of magnetometer expressed in
* Teslas (T).
*
* @param hardIronX x-coordinate of magnetometer
* known hard-iron bias.
* @param hardIronY y-coordinate of magnetometer
* known hard-iron bias.
* @param hardIronZ z-coordinate of magnetometer
* known hard-iron bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final double hardIronX, final double hardIronY, final double hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = hardIronX;
this.hardIronY = hardIronY;
this.hardIronZ = hardIronZ;
}
/**
* Sets known hard-iron coordinates.
*
* @param hardIronX x-coordinate of magnetometer hard-iron.
* @param hardIronY y-coordinate of magnetometer hard-iron.
* @param hardIronZ z-coordinate of magnetometer hard-iron.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIronCoordinates(
final MagneticFluxDensity hardIronX, final MagneticFluxDensity hardIronY,
final MagneticFluxDensity hardIronZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.hardIronX = convertMagneticFluxDensity(hardIronX);
this.hardIronY = convertMagneticFluxDensity(hardIronY);
this.hardIronZ = convertMagneticFluxDensity(hardIronZ);
}
/**
* Gets known hard-iron.
*
* @return known hard-iron.
*/
@Override
public MagneticFluxDensityTriad getHardIronAsTriad() {
return new MagneticFluxDensityTriad(MagneticFluxDensityUnit.TESLA, hardIronX, hardIronY, hardIronZ);
}
/**
* Gets known hard-iron.
*
* @param result instance where result will be stored.
*/
@Override
public void getHardIronAsTriad(final MagneticFluxDensityTriad result) {
result.setValueCoordinatesAndUnit(hardIronX, hardIronY, hardIronZ, MagneticFluxDensityUnit.TESLA);
}
/**
* Sets known hard-iron.
*
* @param hardIron hard-iron to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setHardIron(final MagneticFluxDensityTriad hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
hardIronX = convertMagneticFluxDensity(hardIron.getValueX(), hardIron.getUnit());
hardIronY = convertMagneticFluxDensity(hardIron.getValueY(), hardIron.getUnit());
hardIronZ = convertMagneticFluxDensity(hardIron.getValueZ(), hardIron.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4873 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4265 |
if (estimatedMm == null) {
estimatedMm = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
} else {
estimatedMm.initialize(0.0);
}
estimatedMm.setElementAt(0, 0, sx);
estimatedMm.setElementAt(1, 0, myx);
estimatedMm.setElementAt(2, 0, mzx);
estimatedMm.setElementAt(0, 1, mxy);
estimatedMm.setElementAt(1, 1, sy);
estimatedMm.setElementAt(2, 1, mzy);
estimatedMm.setElementAt(0, 2, mxz);
estimatedMm.setElementAt(1, 2, myz);
estimatedMm.setElementAt(2, 2, sz);
estimatedCovariance = fitter.getCovar();
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Sets input data into Levenberg-Marquardt fitter.
*
* @throws WrongSizeException never happens.
* @throws IOException if world magnetic model cannot be loaded.
*/
private void setInputData() throws WrongSizeException, IOException {
// set input data using:
// mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy = by + myx * mBtruex + mBtruey + sy * mBtruey + myz * mBtruez
// mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + mBtruez + sz * mBtruez
final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
if (magneticModel != null) {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
} else {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
}
final var expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
final var nedFrame = new NEDFrame();
final var earthB = new NEDMagneticFluxDensity();
final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
final var numMeasurements = measurements.size();
final var x = new Matrix(numMeasurements, BodyMagneticFluxDensity.COMPONENTS);
final var y = new Matrix(numMeasurements, BodyMagneticFluxDensity.COMPONENTS);
final var specificForceStandardDeviations = new double[numMeasurements];
var i = 0;
for (final var measurement : measurements) {
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
// estimate Earth magnetic flux density at frame position and
// timestamp using WMM
final var ecefFrame = measurement.getFrame();
ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);
final var year = measurement.getYear();
final var latitude = nedFrame.getLatitude();
final var longitude = nedFrame.getLongitude();
final var height = nedFrame.getHeight();
nedFrame.getCoordinateTransformation(cbn);
cbn.inverse(cnb);
wmmEstimator.estimate(latitude, longitude, height, year, earthB);
// estimate expected body magnetic flux density taking into
// account body attitude (inverse of frame orientation) and
// estimated Earth magnetic flux density
BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);
final var bMeasX = measuredMagneticFluxDensity.getBx();
final var bMeasY = measuredMagneticFluxDensity.getBy();
final var bMeasZ = measuredMagneticFluxDensity.getBz();
final var bTrueX = expectedMagneticFluxDensity.getBx();
final var bTrueY = expectedMagneticFluxDensity.getBy();
final var bTrueZ = expectedMagneticFluxDensity.getBz();
x.setElementAt(i, 0, bTrueX);
x.setElementAt(i, 1, bTrueY);
x.setElementAt(i, 2, bTrueZ);
y.setElementAt(i, 0, bMeasX);
y.setElementAt(i, 1, bMeasY);
y.setElementAt(i, 2, bMeasZ);
specificForceStandardDeviations[i] = measurement.getMagneticFluxDensityStandardDeviation();
i++;
}
fitter.setInputData(x, y, specificForceStandardDeviations);
}
/**
* Converts magnetic flux density value and unit to Teslas.
*
* @param value magnetic flux density value.
* @param unit unit of magnetic flux density value.
* @return converted value.
*/
private static double convertMagneticFluxDensity(final double value, final MagneticFluxDensityUnit unit) {
return MagneticFluxDensityConverter.convert(value, unit, MagneticFluxDensityUnit.TESLA);
}
/**
* Converts magnetic flux density instance to Teslas.
*
* @param magneticFluxDensity magnetic flux density instance to be converted.
* @return converted value.
*/
private static double convertMagneticFluxDensity(final MagneticFluxDensity magneticFluxDensity) {
return convertMagneticFluxDensity(magneticFluxDensity.getValue().doubleValue(), magneticFluxDensity.getUnit());
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4015 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4346 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2083 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2302 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1375 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1331 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 743 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1411 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1367 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2075 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3158 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2079 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3120 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1402 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1478 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3016 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2983 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 772 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 741 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1564 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1529 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1586 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 1551 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 718 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2658 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 1820 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 2901 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 1824 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 2865 |
}
/**
* Gets known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return x coordinate of gyroscope bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasX x coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return y coordinate of gyroscope bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasY y coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return z coordinate of gyroscope bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x coordinate of gyroscope bias.
*
* @return x coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedX() {
return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known x coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(biasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known x coordinate of gyroscope bias.
*
* @param biasX x coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final AngularSpeed biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
}
/**
* Gets known y coordinate of gyroscope bias.
*
* @return y coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedY() {
return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known y coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(biasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known y coordinate of gyroscope bias.
*
* @param biasY y coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final AngularSpeed biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAngularSpeed(biasY);
}
/**
* Gets known z coordinate of gyroscope bias.
*
* @return z coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedZ() {
return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known z coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(biasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known z coordinate of gyroscope bias.
*
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Sets known gyroscope bias coordinates expressed in radians per second
* (rad/s).
*
* @param biasX x coordinate of gyroscope bias.
* @param biasY y coordinate of gyroscope bias.
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
this.biasY = biasY;
this.biasZ = biasZ;
}
/**
* Sets known gyroscope bias coordinates.
*
* @param biasX x coordinate of gyroscope bias.
* @param biasY y coordinate of gyroscope bias.
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
this.biasY = convertAngularSpeed(biasY);
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Gets known gyroscope bias.
*
* @return known gyroscope bias.
*/
public AngularSpeedTriad getBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known gyroscope bias.
*
* @param result instance where result will be stored.
*/
public void getBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known gyroscope bias.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setBias(final AngularSpeedTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinate of known bias.
*/
@Override
public double[] getBias() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4015 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2083 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1375 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1331 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2075 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3158 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2464 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2079 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3120 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 728 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1402 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1478 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3016 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2983 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 772 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 741 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1564 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1529 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1586 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 1551 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4015 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4346 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2083 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2302 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1363 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1375 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1331 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 743 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1411 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1367 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2075 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3158 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2464 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1382 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2079 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3120 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 728 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1402 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1478 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3016 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2983 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 772 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 741 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1564 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1529 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1586 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 1551 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1364 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2114 |
}
/**
* Gets initial x scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2506 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2475 |
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @return collection of sequences of timestamped body kinematics
* measurements.
*/
@Override
public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
return sequences;
}
/**
* Sets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @param sequences collection of sequences of timestamped body
* kinematics measurements.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setSequences(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences) throws LockedException {
if (running) {
throw new LockedException();
}
this.sequences = sequences;
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return true;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Indicates whether G-dependent cross biases are being estimated
* or not.
* When enabled, this adds 9 variables from Gg matrix.
*
* @return true if G-dependent cross biases will be estimated,
* false otherwise.
*/
public boolean isGDependentCrossBiasesEstimated() {
return estimateGDependentCrossBiases;
}
/**
* Specifies whether G-dependent cross biases are being estimated
* or not.
* When enabled, this adds 9 variables from Gg matrix.
*
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setGDependentCrossBiasesEstimated(final boolean estimateGDependentCrossBiases) throws LockedException {
if (running) {
throw new LockedException();
}
this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public EasyGyroscopeCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4016 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4347 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2084 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2303 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1376 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1332 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 744 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1412 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1368 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2114 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2076 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3159 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2465 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2080 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3121 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 729 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1403 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1479 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3017 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2984 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 773 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 742 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1565 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1530 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1587 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 1552 |
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4015 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4346 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2083 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2302 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1363 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1375 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1331 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 743 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1411 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1367 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2072 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2075 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3158 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2069 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1382 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2079 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3120 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3133 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3172 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1402 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1478 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3016 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 2983 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 772 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 741 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1564 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1529 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1586 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 1551 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2073 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2114 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2070 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3134 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3173 |
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() {
return initialSx;
}
/**
* Sets initial x scaling factor of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSx(final double initialSx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
}
/**
* Gets initial y scaling factor of gyroscope.
*
* @return initial y scaling factor of gyroscope.
*/
@Override
public double getInitialSy() {
return initialSy;
}
/**
* Sets initial y scaling factor of gyroscope.
*
* @param initialSy initial y scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSy(final double initialSy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSy = initialSy;
}
/**
* Gets initial z scaling factor of gyroscope.
*
* @return initial z scaling factor of gyroscope.
*/
@Override
public double getInitialSz() {
return initialSz;
}
/**
* Sets initial z scaling factor of gyroscope.
*
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialSz(final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSz = initialSz;
}
/**
* Gets initial x-y cross coupling error of gyroscope.
*
* @return initial x-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMxy() {
return initialMxy;
}
/**
* Sets initial x-y cross coupling error of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxy(final double initialMxy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
}
/**
* Gets initial x-z cross coupling error of gyroscope.
*
* @return initial x-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMxz() {
return initialMxz;
}
/**
* Sets initial x-z cross coupling error of gyroscope.
*
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMxz(final double initialMxz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxz = initialMxz;
}
/**
* Gets initial y-x cross coupling error of gyroscope.
*
* @return initial y-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMyx() {
return initialMyx;
}
/**
* Sets initial y-x cross coupling error of gyroscope.
*
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyx(final double initialMyx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyx = initialMyx;
}
/**
* Gets initial y-z cross coupling error of gyroscope.
*
* @return initial y-z cross coupling error of gyroscope.
*/
@Override
public double getInitialMyz() {
return initialMyz;
}
/**
* Sets initial y-z cross coupling error of gyroscope.
*
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMyz(final double initialMyz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMyz = initialMyz;
}
/**
* Gets initial z-x cross coupling error of gyroscope.
*
* @return initial z-x cross coupling error of gyroscope.
*/
@Override
public double getInitialMzx() {
return initialMzx;
}
/**
* Sets initial z-x cross coupling error of gyroscope.
*
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzx(final double initialMzx) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzx = initialMzx;
}
/**
* Gets initial z-y cross coupling error of gyroscope.
*
* @return initial z-y cross coupling error of gyroscope.
*/
@Override
public double getInitialMzy() {
return initialMzy;
}
/**
* Sets initial z-y cross coupling error of gyroscope.
*
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMzy(final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors of gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactors(
final double initialSx, final double initialSy, final double initialSz) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialSx = initialSx;
this.initialSy = initialSy;
this.initialSz = initialSz;
}
/**
* Sets initial cross coupling errors of gyroscope.
*
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialMxy = initialMxy;
this.initialMxz = initialMxz;
this.initialMyx = initialMyx;
this.initialMyz = initialMyz;
this.initialMzx = initialMzx;
this.initialMzy = initialMzy;
}
/**
* Sets initial scaling factors and cross coupling errors of
* gyroscope.
*
* @param initialSx initial x scaling factor of gyroscope.
* @param initialSy initial y scaling factor of gyroscope.
* @param initialSz initial z scaling factor of gyroscope.
* @param initialMxy initial x-y cross coupling error of gyroscope.
* @param initialMxz initial x-z cross coupling error of gyroscope.
* @param initialMyx initial y-x cross coupling error of gyroscope.
* @param initialMyz initial y-z cross coupling error of gyroscope.
* @param initialMzx initial z-x cross coupling error of gyroscope.
* @param initialMzy initial z-y cross coupling error of gyroscope.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException {
if (running) {
throw new LockedException();
}
setInitialScalingFactors(initialSx, initialSy, initialSz);
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1957 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2066 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2024 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
* This is only available when result has been refined and covariance
* is kept.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1443 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2171 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2305 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS}.
*
* @return size of subsets to be checked during robust estimation.
*/
public int getPreliminarySubsetSize() {
return preliminarySubsetSize;
}
/**
* Sets size of subsets to be checked during robust estimation.
* This has to be at least {@link #MINIMUM_MEASUREMENTS}.
*
* @param preliminarySubsetSize size of subsets to be checked during robust estimation.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided value is less than {@link #MINIMUM_MEASUREMENTS}.
*/
public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
if (running) {
throw new LockedException();
}
if (preliminarySubsetSize < MINIMUM_MEASUREMENTS) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2502 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2476 |
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @return collection of sequences of timestamped body kinematics
* measurements.
*/
@Override
public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
return sequences;
}
/**
* Sets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @param sequences collection of sequences of timestamped body
* kinematics measurements.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setSequences(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences) throws LockedException {
if (running) {
throw new LockedException();
}
this.sequences = sequences;
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Indicates whether G-dependent cross biases are being estimated
* or not.
* When enabled, this adds 9 variables from Gg matrix.
*
* @return true if G-dependent cross biases will be estimated,
* false otherwise.
*/
public boolean isGDependentCrossBiasesEstimated() {
return estimateGDependentCrossBiases;
}
/**
* Specifies whether G-dependent cross biases are being estimated
* or not.
* When enabled, this adds 9 variables from Gg matrix.
*
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setGDependentCrossBiasesEstimated(final boolean estimateGDependentCrossBiases) throws LockedException {
if (running) {
throw new LockedException();
}
this.estimateGDependentCrossBiases = estimateGDependentCrossBiases;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public RobustEasyGyroscopeCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1744 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1791 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @return position where body kinematics measures have been taken.
*/
public ECEFPosition getEcefPosition() {
return position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final ECEFPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @return position where body kinematics measures have been taken or null if
* not available.
*/
public NEDPosition getNedPosition() {
final var result = new NEDPosition();
return getNedPosition(result) ? result : null;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param result instance where result will be stored.
* @return true if NED position could be computed, false otherwise.
*/
public boolean getNedPosition(final NEDPosition result) {
if (position != null) {
final var velocity = new NEDVelocity();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(position.getX(), position.getY(), position.getZ(),
0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final NEDPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = convertPosition(position);
}
/**
* Gets a list of body kinematics measurements taken at a
* given position with different unknown orientations and containing the
* standard deviations of accelerometer and gyroscope measurements.
*
* @return list of body kinematics measurements.
*/
@Override
public List<StandardDeviationBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a list of body kinematics measurements taken at a
* given position with different unknown orientations and containing the
* standard deviations of accelerometer and gyroscope measurements.
*
* @param measurements list of body kinematics measurements.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final List<StandardDeviationBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public AccelerometerCalibratorMeasurementType getMeasurementType() {
return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public RobustKnownBiasAndPositionAccelerometerCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2401 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2899 |
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<StandardDeviationFrameBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @param measurements collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(
final Collection<? extends StandardDeviationFrameBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
//noinspection unchecked
this.measurements = (Collection<StandardDeviationFrameBodyKinematics>) measurements;
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_FRAME_BODY_KINEMATICS_MEASUREMENT;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mg matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mg matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this calibrator.
*
* @return listener to handle events raised by this calibrator.
*/
@Override
public KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2039 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 469 |
this(measurements, commonAxisUsed, initialBias, initialMa);
this.listener = listener;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasXAsAcceleration() {
return new Acceleration(initialBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasXAsAcceleration(final Acceleration result) {
result.setValue(initialBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final Acceleration initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAcceleration(initialBiasX);
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasYAsAcceleration() {
return new Acceleration(initialBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasYAsAcceleration(final Acceleration result) {
result.setValue(initialBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final Acceleration initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAcceleration(initialBiasY);
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasZAsAcceleration() {
return new Acceleration(initialBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasZAsAcceleration(final Acceleration result) {
result.setValue(initialBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final Acceleration initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAcceleration(initialBiasZ);
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution
* expressed in meters per squared second (m/s^2).
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(
final double initialBiasX, final double initialBiasY, final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(
final Acceleration initialBiasX, final Acceleration initialBiasY, final Acceleration initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAcceleration(initialBiasX);
this.initialBiasY = convertAcceleration(initialBiasY);
this.initialBiasZ = convertAcceleration(initialBiasZ);
}
/**
* Gets initial bias coordinates of accelerometer used to find a solution.
*
* @return initial bias coordinates.
*/
@Override
public AccelerationTriad getInitialBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of accelerometer used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution.
*
* @param initialBias initial bias coordinates to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final AccelerationTriad initialBias) throws LockedException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1790 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1835 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets ground truth gravity norm to be expected at location where measurements have been made,
* expressed in meter per squared second (m/s^2).
*
* @return ground truth gravity norm or null.
*/
public Double getGroundTruthGravityNorm() {
return groundTruthGravityNorm;
}
/**
* Sets ground truth gravity norm to be expected at location where
* measurements have been made, expressed in meters per squared second
* (m/s^2).
*
* @param groundTruthGravityNorm ground truth gravity norm or
* null if undefined.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if calibrator is currently running.
*/
public void setGroundTruthGravityNorm(final Double groundTruthGravityNorm) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
internalSetGroundTruthGravityNorm(groundTruthGravityNorm);
}
/**
* Gets ground truth gravity norm to be expected at location where measurements have been made.
*
* @return ground truth gravity norm or null.
*/
public Acceleration getGroundTruthGravityNormAsAcceleration() {
return groundTruthGravityNorm != null
? new Acceleration(groundTruthGravityNorm, AccelerationUnit.METERS_PER_SQUARED_SECOND) : null;
}
/**
* Gets ground truth gravity norm to be expected at location where measurements have been made.
*
* @param result instance where result will be stored.
* @return true if ground truth gravity norm has been defined, false if it is not available yet.
*/
public boolean getGroundTruthGravityNormAsAcceleration(final Acceleration result) {
if (groundTruthGravityNorm != null) {
result.setValue(groundTruthGravityNorm);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
return true;
} else {
return false;
}
}
/**
* Sets ground truth gravity norm to be expected at location where
* measurements have been made.
*
* @param groundTruthGravityNorm ground truth gravity norm or null
* if undefined.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if calibrator is currently running.
*/
public void setGroundTruthGravityNorm(final Acceleration groundTruthGravityNorm) throws LockedException {
setGroundTruthGravityNorm(groundTruthGravityNorm != null ? convertAcceleration(groundTruthGravityNorm) : null);
}
/**
* Gets a list of body kinematics measurements taken at a
* given position with different unknown orientations and containing the
* standard deviations of accelerometer and gyroscope measurements.
*
* @return list of body kinematics measurements.
*/
@Override
public List<StandardDeviationBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a list of body kinematics measurements taken at a
* given position with different unknown orientations and containing the
* standard deviations of accelerometer and gyroscope measurements.
*
* @param measurements list of body kinematics measurements.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final List<StandardDeviationBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public AccelerometerCalibratorMeasurementType getMeasurementType() {
return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public RobustKnownBiasAndGravityNormAccelerometerCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 848 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1931 |
return false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Internal method to perform calibration when common z-axis is assumed
* for the accelerometer, gyroscope and magnetometer.
*
* @throws AlgebraException if there are numerical errors.
* @throws IOException if world magnetic model cannot be loaded.
*/
private void calibrateCommonAxis() throws AlgebraException, IOException {
// The magnetometer model is:
// mBmeas = bm + (I + Mm) * mBtrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// mBmeas = bm + (I + Mm) * mBtrue
// Hence:
// [mBmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [mBtruex]
// [mBmeasy] = [by] [0 1 0] [myx sy myz] [mBtruey]
// [mBmeasz] = [bz] [0 0 1] [mzx mzy sz ] [mBtruez]
// where myx = mzx = mzy = 0
// Hence:
// [mBmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [mBtruex]
// [mBmeasy] = [by] [0 1 0] [0 sy myz] [mBtruey]
// [mBmeasz] = [bz] [0 0 1] [0 0 sz ] [mBtruez]
// [mBmeasx] = [bx] + [1+sx mxy mxz ][mBtruex]
// [mBmeasy] [by] [0 1+sy myz ][mBtruey]
// [mBmeasz] [bz] [0 0 1+sz][mBtruez]
// mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy = by + (1+sy) * mBtruey + myz * mBtruez
// mBmeasz = bz + (1+sz) * mBtruez
// Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myz
// Reordering:
// mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy = by + mBtruey + sy * mBtruey + myz * mBtruez
// mBmeasz = bz + mBtruez + sz * mBtruez
// mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy - mBtruey = by + sy * mBtruey + myz * mBtruez
// mBmeasz - mBtruez = bz + sz * mBtruez
// [1 0 0 mBtruex 0 0 mBtruey mBtruez 0 ][bx ] = [mBmeasx - mBtruex]
// [0 1 0 0 mBtruey 0 0 0 mBtruez][by ] [mBmeasy - mBtruey]
// [0 0 1 0 0 mBtruez 0 0 0 ][bz ] [mBmeasz - mBtruez]
// [sx ]
// [sy ]
// [sz ]
// [mxy]
// [mxz]
// [myz]
final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
if (magneticModel != null) {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
} else {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
}
final var expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
final var nedFrame = new NEDFrame();
final var earthB = new NEDMagneticFluxDensity();
final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
// estimate Earth magnetic flux density at frame position and
// timestamp using WMM
final var ecefFrame = measurement.getFrame();
ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);
final var year = measurement.getYear();
final var latitude = nedFrame.getLatitude();
final var longitude = nedFrame.getLongitude();
final var height = nedFrame.getHeight();
nedFrame.getCoordinateTransformation(cbn);
cbn.inverse(cnb);
wmmEstimator.estimate(latitude, longitude, height, year, earthB);
// estimate expected body magnetic flux density taking into
// account body attitude (inverse of frame orientation) and
// estimated Earth magnetic flux density
BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);
final var bMeasX = measuredMagneticFluxDensity.getBx();
final var bMeasY = measuredMagneticFluxDensity.getBy();
final var bMeasZ = measuredMagneticFluxDensity.getBz();
final var bTrueX = expectedMagneticFluxDensity.getBx();
final var bTrueY = expectedMagneticFluxDensity.getBy();
final var bTrueZ = expectedMagneticFluxDensity.getBz();
a.setElementAt(i, 0, 1.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanInitializerConfig.java | 112 |
| com/irurueta/navigation/inertial/INSTightlyCoupledKalmanInitializerConfig.java | 132 |
public INSLooselyCoupledKalmanInitializerConfig(final INSLooselyCoupledKalmanInitializerConfig input) {
copyFrom(input);
}
/**
* Gets initial attitude uncertainty per axis expressed in radians (rad).
*
* @return initial attitude uncertainty per axis expressed in radians (rad).
*/
public double getInitialAttitudeUncertainty() {
return initialAttitudeUncertainty;
}
/**
* Sets initial attitude uncertainty per axis expressed in radians (rad).
*
* @param initialAttitudeUncertainty initial attitude uncertainty per axis expressed
* in radians (rad).
*/
public void setInitialAttitudeUncertainty(final double initialAttitudeUncertainty) {
this.initialAttitudeUncertainty = initialAttitudeUncertainty;
}
/**
* Gets initial attitude uncertainty per axis.
*
* @param result instance where initial attitude uncertainty per axis will be stored.
*/
public void getInitialAttitudeUncertaintyAngle(final Angle result) {
result.setValue(initialAttitudeUncertainty);
result.setUnit(AngleUnit.RADIANS);
}
/**
* Gets initial attitude uncertainty per axis.
*
* @return initial attitude uncertainty per axis.
*/
public Angle getInitialAttitudeUncertaintyAngle() {
return new Angle(initialAttitudeUncertainty, AngleUnit.RADIANS);
}
/**
* Sets initial attitude uncertainty per axis.
*
* @param initialAttitudeUncertainty initial attitude uncertainty per axis.
*/
public void setInitialAttitudeUncertainty(final Angle initialAttitudeUncertainty) {
this.initialAttitudeUncertainty = AngleConverter.convert(initialAttitudeUncertainty.getValue().doubleValue(),
initialAttitudeUncertainty.getUnit(), AngleUnit.RADIANS);
}
/**
* Gets initial velocity uncertainty per axis expressed in meters per second (m/s).
*
* @return initial velocity uncertainty per axis expressed in meters per second (m/s).
*/
public double getInitialVelocityUncertainty() {
return initialVelocityUncertainty;
}
/**
* Sets initial velocity uncertainty per axis expressed in meters per second (m/s).
*
* @param initialVelocityUncertainty initial velocity uncertainty per axis expressed
* in meters per second (m/s).
*/
public void setInitialVelocityUncertainty(final double initialVelocityUncertainty) {
this.initialVelocityUncertainty = initialVelocityUncertainty;
}
/**
* Gets initial velocity uncertainty per axis.
*
* @param result instance where initial attitude uncertainty per axis will be stored.
*/
public void getInitialVelocityUncertaintySpeed(final Speed result) {
result.setValue(initialVelocityUncertainty);
result.setUnit(SpeedUnit.METERS_PER_SECOND);
}
/**
* Gets initial velocity uncertainty per axis.
*
* @return initial velocity uncertainty per axis.
*/
public Speed getInitialVelocityUncertaintySpeed() {
return new Speed(initialVelocityUncertainty, SpeedUnit.METERS_PER_SECOND);
}
/**
* Sets initial velocity uncertainty per axis.
*
* @param initialVelocityUncertainty initial velocity uncertainty per axis.
*/
public void setInitialVelocityUncertainty(final Speed initialVelocityUncertainty) {
this.initialVelocityUncertainty = SpeedConverter.convert(initialVelocityUncertainty.getValue().doubleValue(),
initialVelocityUncertainty.getUnit(), SpeedUnit.METERS_PER_SECOND);
}
/**
* Gets initial position uncertainty per axis expressed in meters (m)
*
* @return initial position uncertainty per axis expressed in meters (m).
*/
public double getInitialPositionUncertainty() {
return initialPositionUncertainty;
}
/**
* Sets initial position uncertainty per axis expressed in meters (m)
*
* @param initialPositionUncertainty initial position uncertainty per axis expressed
* in meters (m).
*/
public void setInitialPositionUncertainty(final double initialPositionUncertainty) {
this.initialPositionUncertainty = initialPositionUncertainty;
}
/**
* Gets initial position uncertainty per axis.
*
* @param result instance where initial position uncertainty per axis will be stored.
*/
public void getInitialPositionUncertaintyDistance(final Distance result) {
result.setValue(initialPositionUncertainty);
result.setUnit(DistanceUnit.METER);
}
/**
* Gets initial position uncertainty per axis.
*
* @return initial position uncertainty per axis.
*/
public Distance getInitialPositionUncertaintyDistance() {
return new Distance(initialPositionUncertainty, DistanceUnit.METER);
}
/**
* Sets initial position uncertainty per axis.
*
* @param initialPositionUncertainty initial position uncertainty per axis.
*/
public void setInitialPositionUncertainty(final Distance initialPositionUncertainty) {
this.initialPositionUncertainty = DistanceConverter.convert(initialPositionUncertainty.getValue().doubleValue(),
initialPositionUncertainty.getUnit(), DistanceUnit.METER);
}
/**
* Gets initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
*
* @return initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
*/
public double getInitialAccelerationBiasUncertainty() {
return initialAccelerationBiasUncertainty;
}
/**
* Sets initial acceleration bias uncertainty expressed in meters per squared second (m/s^2).
*
* @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty expressed in
* meters per squared second (m/s^2).
*/
public void setInitialAccelerationBiasUncertainty(final double initialAccelerationBiasUncertainty) {
this.initialAccelerationBiasUncertainty = initialAccelerationBiasUncertainty;
}
/**
* Gets initial acceleration bias uncertainty.
*
* @param result instance where initial acceleration bias uncertainty will be stored.
*/
public void getInitialAccelerationBiasUncertaintyAcceleration(final Acceleration result) {
result.setValue(initialAccelerationBiasUncertainty);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial acceleration bias uncertainty.
*
* @return initial acceleration bias uncertainty.
*/
public Acceleration getInitialAccelerationBiasUncertaintyAcceleration() {
return new Acceleration(initialAccelerationBiasUncertainty, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial acceleration bias uncertainty.
*
* @param initialAccelerationUncertainty initial acceleration bias uncertainty.
*/
public void setInitialAccelerationBiasUncertainty(final Acceleration initialAccelerationUncertainty) {
initialAccelerationBiasUncertainty = AccelerationConverter.convert(
initialAccelerationUncertainty.getValue().doubleValue(), initialAccelerationUncertainty.getUnit(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial gyroscope bias uncertainty expressed in radians per second (rad/s).
*
* @return initial gyroscope bias uncertainty expressed in radians per second (rad/s).
*/
public double getInitialGyroscopeBiasUncertainty() {
return initialGyroscopeBiasUncertainty;
}
/**
* Sets initial gyroscope bias uncertainty expressed in radians per second (rad/s).
*
* @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty expressed
* in radians per second (rad/s).
*/
public void setInitialGyroscopeBiasUncertainty(final double initialGyroscopeBiasUncertainty) {
this.initialGyroscopeBiasUncertainty = initialGyroscopeBiasUncertainty;
}
/**
* Gets initial gyroscope bias uncertainty.
*
* @param result instance where initial gyroscope bias uncertainty will be stored.
*/
public void getInitialGyroscopeBiasUncertaintyAngularSpeed(final AngularSpeed result) {
result.setValue(initialGyroscopeBiasUncertainty);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets initial gyroscope bias uncertainty.
*
* @return initial gyroscope bias uncertainty.
*/
public AngularSpeed getInitialGyroscopeBiasUncertaintyAngularSpeed() {
return new AngularSpeed(initialGyroscopeBiasUncertainty, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets initial gyroscope bias uncertainty.
*
* @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty.
*/
public void setInitialGyroscopeBiasUncertainty(final AngularSpeed initialGyroscopeBiasUncertainty) {
this.initialGyroscopeBiasUncertainty = AngularSpeedConverter.convert(
initialGyroscopeBiasUncertainty.getValue().doubleValue(), initialGyroscopeBiasUncertainty.getUnit(),
AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets configuration parameters.
*
* @param initialAttitudeUncertainty initial attitude uncertainty per axis
* expressed in radians (rad).
* @param initialVelocityUncertainty initial velocity uncertainty per axis
* expressed in meters per second (m/s).
* @param initialPositionUncertainty initial position uncertainty per axis
* expressed in meters (m).
* @param initialAccelerationBiasUncertainty initial acceleration bias uncertainty
* expressed in meters per squared second (m/s^2).
* @param initialGyroscopeBiasUncertainty initial gyroscope bias uncertainty
* expressed in radians per second (rad/s).
*/
public void setValues( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4086 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2041 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 471 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1138 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1094 |
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasXAsAcceleration() {
return new Acceleration(initialBiasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasXAsAcceleration(final Acceleration result) {
result.setValue(initialBiasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final Acceleration initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAcceleration(initialBiasX);
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasYAsAcceleration() {
return new Acceleration(initialBiasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasYAsAcceleration(final Acceleration result) {
result.setValue(initialBiasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final Acceleration initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = convertAcceleration(initialBiasY);
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasZAsAcceleration() {
return new Acceleration(initialBiasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
*
* @param result instance where result data will be stored.
*/
@Override
public void getInitialBiasZAsAcceleration(final Acceleration result) {
result.setValue(initialBiasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final Acceleration initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = convertAcceleration(initialBiasZ);
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution
* expressed in meters per squared second (m/s^2).
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final double initialBiasX, final double initialBiasY, final double initialBiasZ)
throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
this.initialBiasY = initialBiasY;
this.initialBiasZ = initialBiasZ;
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final Acceleration initialBiasX, final Acceleration initialBiasY,
final Acceleration initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = convertAcceleration(initialBiasX);
this.initialBiasY = convertAcceleration(initialBiasY);
this.initialBiasZ = convertAcceleration(initialBiasZ);
}
/**
* Gets initial bias coordinates of accelerometer used to find a solution.
*
* @return initial bias coordinates.
*/
@Override
public AccelerationTriad getInitialBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND,
initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of accelerometer used to find a solution.
*
* @param result instance where result will be stored.
*/
@Override
public void getInitialBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(initialBiasX, initialBiasY, initialBiasZ,
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets initial bias coordinates of accelerometer used to find a solution.
*
* @param initialBias initial bias coordinates to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBias(final AccelerationTriad initialBias) throws LockedException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1184 |
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets a list of body kinematics measurements taken at different
* frames (positions, orientations and velocities) and containing the standard
* deviations of accelerometer and gyroscope measurements.
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public List<StandardDeviationFrameBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a list of body kinematics measurements taken at different
* frames (positions, orientations and velocities) and containing the standard
* deviations of accelerometer and gyroscope measurements.
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @param measurements collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final List<StandardDeviationFrameBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_FRAME_BODY_KINEMATICS_MEASUREMENT;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public RobustKnownBiasAndFrameGyroscopeCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2020 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2777 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4013 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated calibration solution.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
* This is only available when result has been refined and covariance
* is kept.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 1973 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1103 |
final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame, final BodyKinematics result) {
estimateKinematics(timeInterval, frame, oldFrame.getCoordinateTransformation(),
oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final double x, final double y, final double z, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC,
SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
x, y, z, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final double x, final double y, final double z, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC,
SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
x, y, z, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param position cartesian body position resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1374 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1435 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2475 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2476 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @return collection of sequences of timestamped body kinematics
* measurements.
*/
@Override
public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
return sequences;
}
/**
* Sets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @param sequences collection of sequences of timestamped body
* kinematics measurements.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setSequences(final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences)
throws LockedException {
if (running) {
throw new LockedException();
}
this.sequences = sequences;
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return true;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2506 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2476 |
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @return collection of sequences of timestamped body kinematics
* measurements.
*/
@Override
public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
return sequences;
}
/**
* Sets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @param sequences collection of sequences of timestamped body
* kinematics measurements.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setSequences(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences) throws LockedException {
if (running) {
throw new LockedException();
}
this.sequences = sequences;
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return true;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2475 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2502 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @return collection of sequences of timestamped body kinematics
* measurements.
*/
@Override
public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() {
return sequences;
}
/**
* Sets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @param sequences collection of sequences of timestamped body
* kinematics measurements.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setSequences(final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences)
throws LockedException {
if (running) {
throw new LockedException();
}
this.sequences = sequences;
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.BODY_KINEMATICS_SEQUENCE;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return true;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java | 1167 |
| com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java | 1313 |
}
/**
* Gets body to ECEF coordinate transformation.
*
* @param threshold threshold to determine whether current body to ECEF transformation
* matrix is valid or not (to check that matrix is 3x3 orthonormal).
* @return body to ECEF coordinate transformation.
* @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
* is considered not valid (is not a 3x3 orthonormal matrix) with provided threshold.
*/
public CoordinateTransformation getC(final double threshold) throws InvalidRotationMatrixException {
return bodyToEcefCoordinateTransformationMatrix != null
? new CoordinateTransformation(bodyToEcefCoordinateTransformationMatrix, FrameType.BODY_FRAME,
FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME, threshold)
: null;
}
/**
* Gets body to ECEF coordinate transformation.
*
* @param result instance where body to ECEF coordinate transformation will be stored.
* @return true if result instance was updated, false otherwise.
* @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
* is not valid (is not a 3x3 orthonormal matrix).
*/
public boolean getC(final CoordinateTransformation result) throws InvalidRotationMatrixException {
if (bodyToEcefCoordinateTransformationMatrix != null) {
result.setSourceType(FrameType.BODY_FRAME);
result.setDestinationType(FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME);
result.setMatrix(bodyToEcefCoordinateTransformationMatrix);
return true;
} else {
return false;
}
}
/**
* Gets body to ECEF coordinate transformation.
*
* @param result instance where body to ECEF coordinate transformation will be stored.
* @param threshold threshold to determine whether current body to ECEF transformation
* matrix is valid or not (to check that matrix is 3x3 orthonormal).
* @return true if result instance was updated, false otherwise.
* @throws InvalidRotationMatrixException if current body to ECEF transformation matrix
* is not valid (is not a 3x3 orthonormal matrix) with provided threshold.
*/
public boolean getC(final CoordinateTransformation result, final double threshold)
throws InvalidRotationMatrixException {
if (bodyToEcefCoordinateTransformationMatrix != null) {
result.setSourceType(FrameType.BODY_FRAME);
result.setDestinationType(FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME);
result.setMatrix(bodyToEcefCoordinateTransformationMatrix, threshold);
return true;
} else {
return false;
}
}
/**
* Sets body to ECEF coordinate transformation.
*
* @param c body to ECEF coordinate transformation to be set.
* @throws IllegalArgumentException if provided coordinate transformation is
* not null and is not a body to ECEF transformation.
*/
public void setC(final CoordinateTransformation c) {
if (c == null) {
bodyToEcefCoordinateTransformationMatrix = null;
} else {
if (c.getSourceType() != FrameType.BODY_FRAME
|| c.getDestinationType() != FrameType.EARTH_CENTERED_EARTH_FIXED_FRAME) {
throw new IllegalArgumentException();
}
if (bodyToEcefCoordinateTransformationMatrix != null) {
c.getMatrix(bodyToEcefCoordinateTransformationMatrix);
} else {
bodyToEcefCoordinateTransformationMatrix = c.getMatrix();
}
}
}
/**
* Gets estimated ECEF user velocity resolved around x axis.
*
* @param result instance where estimated ECEF user velocity resolved around x axis will be stored.
*/
public void getSpeedX(final Speed result) {
result.setValue(vx);
result.setUnit(SpeedUnit.METERS_PER_SECOND);
}
/**
* Gets estimated ECEF user velocity resolved around x axis.
*
* @return estimated ECEF user velocity resolved around x axis.
*/
public Speed getSpeedX() {
return new Speed(vx, SpeedUnit.METERS_PER_SECOND);
}
/**
* Sets estimated ECEF user velocity resolved around x axis.
*
* @param vx estimated ECEF user velocity resolved around x axis.
*/
public void setSpeedX(final Speed vx) {
this.vx = SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND);
}
/**
* Gets estimated ECEF user velocity resolved around y axis.
*
* @param result instance where estimated ECEF user velocity resolved around y axis will be stored.
*/
public void getSpeedY(final Speed result) {
result.setValue(vy);
result.setUnit(SpeedUnit.METERS_PER_SECOND);
}
/**
* Gets estimated ECEF user velocity resolved around y axis.
*
* @return estimated ECEF velocity resolved around y axis.
*/
public Speed getSpeedY() {
return new Speed(vy, SpeedUnit.METERS_PER_SECOND);
}
/**
* Sets estimated ECEF user velocity resolved around y axis.
*
* @param vy estimated ECEF user velocity resolved around y axis.
*/
public void setSpeedY(final Speed vy) {
this.vy = SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND);
}
/**
* Gets estimated ECEF user velocity resolved around z axis.
*
* @param result instance where estimated ECEF user velocity resolved around z axis will be stored.
*/
public void getSpeedZ(final Speed result) {
result.setValue(vz);
result.setUnit(SpeedUnit.METERS_PER_SECOND);
}
/**
* Gets estimated ECEF user velocity resolved around z axis.
*
* @return estimated ECEF velocity resolved around z axis.
*/
public Speed getSpeedZ() {
return new Speed(vz, SpeedUnit.METERS_PER_SECOND);
}
/**
* Sets estimated ECEF user velocity resolved around z axis.
*
* @param vz estimated ECEF velocity resolved around z axis.
*/
public void setSpeedZ(final Speed vz) {
this.vz = SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND);
}
/**
* Sets estimated ECEF user velocity.
*
* @param vx estimated ECEF velocity resolved around x axis.
* @param vy estimated ECEF velocity resolved around y axis.
* @param vz estimated ECEF velocity resolved around z axis.
*/
public void setVelocityCoordinates(final Speed vx, final Speed vy, final Speed vz) {
setSpeedX(vx);
setSpeedY(vy);
setSpeedZ(vz);
}
/**
* Gets estimated ECEF user velocity.
*
* @param result instance where estimated ECEF user velocity will be stored.
*/
public void getEcefVelocity(final ECEFVelocity result) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java | 764 |
| com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java | 878 |
public INSLooselyCoupledKalmanState(final INSLooselyCoupledKalmanState input) {
copyFrom(input);
}
/**
* Gets estimated body to ECEF coordinate transformation matrix.
*
* @return estimated body to ECEF coordinate transformation matrix.
*/
public Matrix getBodyToEcefCoordinateTransformationMatrix() {
return bodyToEcefCoordinateTransformationMatrix;
}
/**
* Sets estimated body to ECEF coordinate transformation matrix.
*
* @param bodyToEcefCoordinateTransformationMatrix estimated body to ECEF coordinate
* transformation matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
public void setBodyToEcefCoordinateTransformationMatrix(final Matrix bodyToEcefCoordinateTransformationMatrix) {
if (bodyToEcefCoordinateTransformationMatrix.getRows() != CoordinateTransformation.ROWS
|| bodyToEcefCoordinateTransformationMatrix.getColumns() != CoordinateTransformation.COLS) {
throw new IllegalArgumentException();
}
this.bodyToEcefCoordinateTransformationMatrix = bodyToEcefCoordinateTransformationMatrix;
}
/**
* Gets estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
*
* @return estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
*/
public double getVx() {
return vx;
}
/**
* Sets estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
*
* @param vx estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
*/
public void setVx(final double vx) {
this.vx = vx;
}
/**
* Gets estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
*
* @return estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
*/
public double getVy() {
return vy;
}
/**
* Sets estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
*
* @param vy estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
*/
public void setVy(final double vy) {
this.vy = vy;
}
/**
* Gets estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
*
* @return estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
*/
public double getVz() {
return vz;
}
/**
* Sets estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
*
* @param vz estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
*/
public void setVz(final double vz) {
this.vz = vz;
}
/**
* Sets estimated ECEF user velocity coordinates.
*
* @param vx estimated ECEF user velocity resolved around x axis and expressed in meters per second (m/s).
* @param vy estimated ECEF user velocity resolved around y axis and expressed in meters per second (m/s).
* @param vz estimated ECEF user velocity resolved around z axis and expressed in meters per second (m/s).
*/
public void setVelocityCoordinates(final double vx, final double vy, final double vz) {
this.vx = vx;
this.vy = vy;
this.vz = vz;
}
/**
* Gets x coordinate of estimated ECEF user position expressed in meters (m).
*
* @return x coordinate of estimated ECEF user position expressed in meters (m).
*/
public double getX() {
return x;
}
/**
* Sets x coordinate of estimated ECEF user position expressed in meters (m).
*
* @param x x coordinate of estimated ECEF user position expressed in meters (m).
*/
public void setX(final double x) {
this.x = x;
}
/**
* Gets y coordinate of estimated ECEF user position expressed in meters (m).
*
* @return y coordinate of estimated ECEF user position expressed in meters (m).
*/
public double getY() {
return y;
}
/**
* Sets y coordinate of estimated ECEF user position expressed in meters (m).
*
* @param y y coordinate of estimated ECEF user position expressed in meters (m).
*/
public void setY(final double y) {
this.y = y;
}
/**
* Gets z coordinate of estimated ECEF user position expressed in meters (m).
*
* @return z coordinate of estimated ECEF user position expressed in meters (m).
*/
public double getZ() {
return z;
}
/**
* Sets z coordinate of estimated ECEF user position expressed in meters (m).
*
* @param z z coordinate of estimated ECEF user position expressed in meters (m).
*/
public void setZ(final double z) {
this.z = z;
}
/**
* Sets estimated ECEF user position coordinates.
*
* @param x x coordinate of estimated ECEF user position expressed in meters (m).
* @param y y coordinate of estimated ECEF user position expressed in meters (m).
* @param z z coordinate of estimated ECEF user position expressed in meters (m).
*/
public void setPositionCoordinates(final double x, final double y, final double z) {
this.x = x;
this.y = y;
this.z = z;
}
/**
* Gets estimated accelerometer bias resolved around x axis and expressed in
* meters per squared second (m/s^2).
*
* @return estimated accelerometer bias resolved around x axis and expressed in
* meters per squared second (m/s^2).
*/
public double getAccelerationBiasX() {
return accelerationBiasX;
}
/**
* Sets estimated accelerometer bias resolved around x axis and expressed in
* meters per squared second (m/s^2).
*
* @param accelerationBiasX estimated accelerometer bias resolved around x axis and
* expressed in meters per squared second (m/s^2).
*/
public void setAccelerationBiasX(final double accelerationBiasX) {
this.accelerationBiasX = accelerationBiasX;
}
/**
* Gets estimated accelerometer bias resolved around y axis and expressed in
* meters per squared second (m/s^2).
*
* @return estimated accelerometer bias resolved around y axis and expressed in
* meters per squared second (m/s^2).
*/
public double getAccelerationBiasY() {
return accelerationBiasY;
}
/**
* Sets estimated accelerometer bias resolved around y axis and expressed in
* meters per squared second (m/s^2).
*
* @param accelerationBiasY estimated accelerometer bias resolved around y axis
* and expressed in meters per squared second (m/s^2).
*/
public void setAccelerationBiasY(final double accelerationBiasY) {
this.accelerationBiasY = accelerationBiasY;
}
/**
* Gets estimated accelerometer bias resolved around z axis and expressed in
* meters per squared second (m/s^2).
*
* @return estimated accelerometer bias resolved around z axis and
* expressed in meters per squared second (m/s^2).
*/
public double getAccelerationBiasZ() {
return accelerationBiasZ;
}
/**
* Sets estimated accelerometer bias resolved around z axis and expressed in
* meters per squared second (m/s^2).
*
* @param accelerationBiasZ estimated accelerometer bias resolved around z axis
* and expressed in meters per squared second (m/s^2).
*/
public void setAccelerationBiasZ(final double accelerationBiasZ) {
this.accelerationBiasZ = accelerationBiasZ;
}
/**
* Sets estimated accelerometer bias expressed in meters per squared second (m/s^2).
*
* @param accelerationBiasX estimated accelerometer bias resolved around x axis and
* expressed in meters per squared second (m/s^2).
* @param accelerationBiasY estimated accelerometer bias resolved around y axis and
* expressed in meters per squared second (m/s^2).
* @param accelerationBiasZ estimated accelerometer bias resolved around z axis and
* expressed in meters per squared second (m/s^2).
*/
public void setAccelerationBiasCoordinates(
final double accelerationBiasX, final double accelerationBiasY, final double accelerationBiasZ) {
this.accelerationBiasX = accelerationBiasX;
this.accelerationBiasY = accelerationBiasY;
this.accelerationBiasZ = accelerationBiasZ;
}
/**
* Gets estimated gyroscope bias resolved around x axis and expressed in
* radians per second (rad/s).
*
* @return estimated gyroscope bias resolved around x axis and expressed in
* radians per second (rad/s).
*/
public double getGyroBiasX() {
return gyroBiasX;
}
/**
* Sets estimated gyroscope bias resolved around x axis and expressed in
* radians per second (rad/s).
*
* @param gyroBiasX estimated gyroscope bias resolved around x axis and
* expressed in radians per second (rad/s).
*/
public void setGyroBiasX(final double gyroBiasX) {
this.gyroBiasX = gyroBiasX;
}
/**
* Gets estimated gyroscope bias resolved around y axis and expressed in
* radians per second (rad/s).
*
* @return estimated gyroscope bias resolved around y axis and expressed
* in radians per second (rad/s).
*/
public double getGyroBiasY() {
return gyroBiasY;
}
/**
* Sets estimated gyroscope bias resolved around y axis and expressed in
* radians per second (rad/s).
*
* @param gyroBiasY estimated gyroscope bias resolved around y axis and
* expressed in radians per second (rad/s).
*/
public void setGyroBiasY(final double gyroBiasY) {
this.gyroBiasY = gyroBiasY;
}
/**
* Gets estimated gyroscope bias resolved around z axis and expressed in
* radians per second (rad/s).
*
* @return estimated gyroscope bias resolved around z axis and expressed
* in radians per second (rad/s).
*/
public double getGyroBiasZ() {
return gyroBiasZ;
}
/**
* Sets estimated gyroscope bias resolved around z axis and expressed in
* radians per second (rad/s).
*
* @param gyroBiasZ estimated gyroscope bias resolved around z axis and
* expressed in radians per second (rad/s).
*/
public void setGyroBiasZ(final double gyroBiasZ) {
this.gyroBiasZ = gyroBiasZ;
}
/**
* Sets estimated gyroscope bias coordinates expressed in radians
* per second (rad/s).
*
* @param gyroBiasX estimated gyroscope bias resolved around x axis and
* expressed in radians per second (rad/s).
* @param gyroBiasY estimated gyroscope bias resolved around y axis and
* expressed in radians per second (rad/s).
* @param gyroBiasZ estimated gyroscope bias resolved around z axis and
* expressed in radians per second (rad/s).
*/
public void setGyroBiasCoordinates(final double gyroBiasX, final double gyroBiasY, final double gyroBiasZ) {
this.gyroBiasX = gyroBiasX;
this.gyroBiasY = gyroBiasY;
this.gyroBiasZ = gyroBiasZ;
}
/**
* Gets Kalman filter error covariance matrix.
* Notice that covariance is expressed in terms of ECEF coordinates.
* If accuracy of position, attitude or velocity needs to be expressed in terms
* of NED coordinates, their respective sub-matrices of this covariance matrix
* must be rotated, taking into account the Jacobian of the matrix transformation
* relating both coordinates, the covariance can be expressed following the law
* of propagation of uncertainties
* <a href="https://en.wikipedia.org/wiki/Propagation_of_uncertainty">(https://en.wikipedia.org/wiki/Propagation_of_uncertainty)</a>
* as: cov(f(x)) = J*cov(x)*J'.
*
* @param result instance where result data will be copied to.
* @return true if result data has been copied, false otherwise.
*/
public boolean getCovariance(final Matrix result) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2499 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2495 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1177 |
public void setInitialBias(AngularSpeedTriad initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @return collection of sequences of timestamped body kinematics
* measurements.
*/
@Override
public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2892 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1177 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3560 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3596 |
public void setInitialBias(final AngularSpeedTriad initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2499 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2892 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2495 |
public void setInitialBias(AngularSpeedTriad initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @return collection of sequences of timestamped body kinematics
* measurements.
*/
@Override
public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2499 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2495 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3560 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3596 |
public void setInitialBias(AngularSpeedTriad initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
initialBiasX = convertAngularSpeed(initialBias.getValueX(), initialBias.getUnit());
initialBiasY = convertAngularSpeed(initialBias.getValueY(), initialBias.getUnit());
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanEpochEstimator.java | 632 |
| com/irurueta/navigation/inertial/INSTightlyCoupledKalmanEpochEstimator.java | 112 |
INSLooselyCoupledKalmanState.NUM_PARAMS, INSLooselyCoupledKalmanState.NUM_PARAMS);
final var tmp1 = omegaIe.multiplyByScalarAndReturnNew(propagationInterval);
final var tmp2 = phiMatrix.getSubmatrix(0, 0, 2, 2);
tmp2.subtract(tmp1);
phiMatrix.setSubmatrix(0, 0, 2, 2, tmp2);
final var estCbeOld = previousState.getBodyToEcefCoordinateTransformationMatrix();
tmp1.copyFrom(estCbeOld);
tmp1.multiplyByScalar(propagationInterval);
phiMatrix.setSubmatrix(0, 12, 2, 14, tmp1);
phiMatrix.setSubmatrix(3, 9, 5, 11, tmp1);
final var measFibb = new Matrix(BodyKinematics.COMPONENTS, 1);
measFibb.setElementAtIndex(0, fx);
measFibb.setElementAtIndex(1, fy);
measFibb.setElementAtIndex(2, fz);
estCbeOld.multiply(measFibb, tmp1);
Utils.skewMatrix(tmp1, tmp2);
tmp2.multiplyByScalar(-propagationInterval);
phiMatrix.setSubmatrix(3, 0, 5, 2, tmp2);
phiMatrix.getSubmatrix(3, 3, 5, 5, tmp1);
tmp2.copyFrom(omegaIe);
tmp2.multiplyByScalar(2.0 * propagationInterval);
tmp1.subtract(tmp2);
phiMatrix.setSubmatrix(3, 3, 5, 5, tmp1);
final var sinPrevLat = Math.sin(previousLatitude);
final var cosPrevLat = Math.cos(previousLatitude);
final var sinPrevLat2 = sinPrevLat * sinPrevLat;
final var cosPrevLat2 = cosPrevLat * cosPrevLat;
// From (2.137)
final var geocentricRadius = EARTH_EQUATORIAL_RADIUS_WGS84
/ Math.sqrt(1.0 - Math.pow(EARTH_ECCENTRICITY * sinPrevLat, 2.0)) * Math.sqrt(cosPrevLat2
+ Math.pow(1.0 - EARTH_ECCENTRICITY * EARTH_ECCENTRICITY, 2.0) * sinPrevLat2);
final var prevX = previousState.getX();
final var prevY = previousState.getY();
final var prevZ = previousState.getZ();
final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(prevX, prevY, prevZ);
final var previousPositionNorm = Math.sqrt(prevX * prevX + prevY * prevY + prevZ * prevZ);
final var estRebeOld = new Matrix(com.irurueta.navigation.frames.ECEFPosition.COMPONENTS, 1);
estRebeOld.setElementAtIndex(0, prevX);
estRebeOld.setElementAtIndex(1, prevY);
estRebeOld.setElementAtIndex(2, prevZ);
final var g = gravity.asMatrix();
g.multiplyByScalar(-2.0 * propagationInterval / geocentricRadius);
final var estRebeOldTrans = estRebeOld.transposeAndReturnNew();
estRebeOldTrans.multiplyByScalar(1.0 / previousPositionNorm);
g.multiply(estRebeOldTrans, tmp1);
phiMatrix.setSubmatrix(3, 6, 5, 8, tmp1);
for (var i = 0; i < com.irurueta.navigation.frames.ECEFPosition.COMPONENTS; i++) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3335 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 4124 |
if (estimatedMa == null) {
estimatedMa = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedMa.initialize(0.0);
}
estimatedMa.setElementAt(0, 0, sx);
estimatedMa.setElementAt(1, 0, myx);
estimatedMa.setElementAt(2, 0, mzx);
estimatedMa.setElementAt(0, 1, mxy);
estimatedMa.setElementAt(1, 1, sy);
estimatedMa.setElementAt(2, 1, mzy);
estimatedMa.setElementAt(0, 2, mxz);
estimatedMa.setElementAt(1, 2, myz);
estimatedMa.setElementAt(2, 2, sz);
estimatedCovariance = fitter.getCovar();
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Sets input data into Levenberg-Marquardt fitter.
*
* @throws WrongSizeException never happens.
*/
private void setInputData() throws WrongSizeException {
// set input data using:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez
final var expectedKinematics = new BodyKinematics();
final var numMeasurements = measurements.size();
final var x = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
final var y = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
final var specificForceStandardDeviations = new double[numMeasurements];
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var fMeasX = measuredKinematics.getFx();
final var fMeasY = measuredKinematics.getFy();
final var fMeasZ = measuredKinematics.getFz();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
x.setElementAt(i, 0, fTrueX);
x.setElementAt(i, 1, fTrueY);
x.setElementAt(i, 2, fTrueZ);
y.setElementAt(i, 0, fMeasX);
y.setElementAt(i, 1, fMeasY);
y.setElementAt(i, 2, fMeasZ);
specificForceStandardDeviations[i] = measurement.getSpecificForceStandardDeviation();
i++;
}
fitter.setInputData(x, y, specificForceStandardDeviations);
}
/**
* Converts acceleration value and unit to meters per squared second.
*
* @param value acceleration value.
* @param unit unit of acceleration value.
* @return converted value.
*/
private static double convertAcceleration(final double value, final AccelerationUnit unit) {
return AccelerationConverter.convert(value, unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Converts acceleration instance to meters per squared second.
*
* @param acceleration acceleration instance to be converted.
* @return converted value.
*/
private static double convertAcceleration(final Acceleration acceleration) {
return convertAcceleration(acceleration.getValue().doubleValue(), acceleration.getUnit());
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1364 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1328 |
final RobustKnownFrameMagnetometerCalibratorListener listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the estimator.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or no.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Gets Earth's magnetic model.
*
* @return Earth's magnetic model or null if not provided.
*/
public WorldMagneticModel getMagneticModel() {
return magneticModel;
}
/**
* Sets Earth's magnetic model.
*
* @param magneticModel Earth's magnetic model to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
if (running) {
throw new LockedException();
}
this.magneticModel = magneticModel;
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @return array containing x,y,z components of estimated magnetometer
* hard-iron biases.
*/
@Override
public double[] getEstimatedHardIron() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1155 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 690 |
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Internal method to perform calibration when common z-axis is assumed for both
* the accelerometer and gyroscope.
*
* @throws AlgebraException if there are numerical errors.
*/
private void calibrateCommonAxis() throws AlgebraException {
// The gyroscope model is:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
// Hence:
// [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1 0] [myx sy myz] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1] [mzx mzy sz ] [Ωtruez] [g31 g32 g33][ftruez]
// where myx = mzx = mzy = 0
// Hence:
// [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1 0] [0 sy myz] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1] [0 0 sz ] [Ωtruez] [g31 g32 g33][ftruez]
// [Ωmeasx] = [bx] + ( [1+sx mxy mxz ]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1+sy myz ] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1+sz] [Ωtruez] [g31 g32 g33][ftruez]
// Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Where the unknowns are: sx, sy, sz, mxy mxz, myz, g11, g12, g13, g21, g22, g23, g31, g32, g33
// Reordering:
// Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy - Ωtruey - by = sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz - Ωtruez - bz = sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// [Ωtruex 0 0 Ωtruey Ωtruez 0 ftruex ftruey ftruez 0 0 0 0 0 0 ][sx ] = [Ωmeasx - Ωtruex - bx]
// [0 Ωtruey 0 0 0 Ωtruez 0 0 0 ftruex ftruey ftruez 0 0 0 ][sy ] [Ωmeasy - Ωtruey - by]
// [0 0 Ωtruez 0 0 0 0 0 0 0 0 0 ftruex ftruey ftruez][sz ] [Ωmeasz - Ωtruez - bz]
// [mxy]
// [mxz]
// [myz]
// [g11]
// [g12]
// [g13]
// [g21]
// [g22]
// [g23]
// [g31]
// [g32]
// [g33]
final var expectedKinematics = new BodyKinematics();
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var omegaMeasX = measuredKinematics.getAngularRateX();
final var omegaMeasY = measuredKinematics.getAngularRateY();
final var omegaMeasZ = measuredKinematics.getAngularRateZ();
final var omegaTrueX = expectedKinematics.getAngularRateX();
final var omegaTrueY = expectedKinematics.getAngularRateY();
final var omegaTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, omegaTrueX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 5144 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4401 |
b.setElementAtIndex(2, bz);
g.setElementAt(0, 0, g11);
g.setElementAt(1, 0, g21);
g.setElementAt(2, 0, g31);
g.setElementAt(0, 1, g12);
g.setElementAt(1, 1, g22);
g.setElementAt(2, 1, g32);
g.setElementAt(0, 2, g13);
g.setElementAt(1, 2, g23);
g.setElementAt(2, 2, g33);
// fix kinematics
final var numItems = measuredSequence.getItemsCount();
final var measuredItems = measuredSequence.getSortedItems();
final var fixedItems = fixedSequence.getSortedItems();
for (var j = 0; j < numItems; j++) {
final var measuredItem = measuredItems.get(j);
final var fixedItem = fixedItems.get(j);
fixKinematics(measuredItem.getKinematics(), fixedItem.getKinematics());
}
// integrate fixed sequence to obtain attitude change
QuaternionIntegrator.integrateGyroSequence(fixedSequence, QuaternionStepIntegratorType.RUNGE_KUTTA, q);
startPoint.setInhomogeneousCoordinates(point[0], point[1], point[2]);
q.inverse();
q.rotate(startPoint, endPoint);
expectedEndPoint.setInhomogeneousCoordinates(point[3], point[4], point[5]);
return expectedEndPoint.distanceTo(endPoint);
} catch (final AlgebraException | RotationException e) {
throw new EvaluationException(e);
}
}
/**
* Fixes provided kinematics with provided accelerometer parameters and
* current gyroscope parameters.
*
* @param kinematics kinematics to be fixed with current values.
* @param result kinematics where result will be stored.
* @throws AlgebraException if for some reason kinematics
*/
private void fixKinematics(final BodyKinematics kinematics, final BodyKinematics result) throws AlgebraException {
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
// Ωmeas = M*(Ωtrue + b + G * ftrue)
// M = I + Mg
// bg = M*b --> b = M^-1*bg
// Gg = M*G --> G = M^-1*Gg
// Ωtrue = M^-1 * Ωmeas - b - G*ftrue
// fix specific force
measuredSpecificForce.setElementAtIndex(0, kinematics.getFx());
measuredSpecificForce.setElementAtIndex(1, kinematics.getFy());
measuredSpecificForce.setElementAtIndex(2, kinematics.getFz());
accelerationFixer.fix(measuredSpecificForce, trueSpecificForce);
// fix angular rate
measuredAngularRate.setElementAtIndex(0, kinematics.getAngularRateX());
measuredAngularRate.setElementAtIndex(1, kinematics.getAngularRateY());
measuredAngularRate.setElementAtIndex(2, kinematics.getAngularRateZ());
g.multiply(trueSpecificForce, tmp);
invM.multiply(measuredAngularRate, trueAngularRate);
trueAngularRate.subtract(b);
trueAngularRate.subtract(tmp);
result.setSpecificForceCoordinates(
trueSpecificForce.getElementAtIndex(0),
trueSpecificForce.getElementAtIndex(1),
trueSpecificForce.getElementAtIndex(2));
result.setAngularRateCoordinates(
trueAngularRate.getElementAtIndex(0),
trueAngularRate.getElementAtIndex(1),
trueAngularRate.getElementAtIndex(2));
}
/**
* Resets any previous estimations.
*/
private void reset() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2477 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2699 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities) and containing the standard
* deviations of accelerometer and gyroscope measurements.
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<StandardDeviationFrameBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities) and containing the standard
* deviations of accelerometer and gyroscope measurements.
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @param measurements collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final Collection<? extends StandardDeviationFrameBodyKinematics> measurements)
throws LockedException {
if (running) {
throw new LockedException();
}
//noinspection unchecked
this.measurements = (Collection<StandardDeviationFrameBodyKinematics>) measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public AccelerometerCalibratorMeasurementType getMeasurementType() {
return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_FRAME_BODY_KINEMATICS;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
@Override
public KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3417 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3379 |
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
return measurements;
}
/**
* Sets a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @param measurements collection of body magnetic flux density measurements
* taken at different frames (positions, orientations
* and velocities).
* @throws LockedException if estimator is currently running.
*/
@Override
public void setMeasurements(final Collection<? extends StandardDeviationFrameBodyMagneticFluxDensity> measurements)
throws LockedException {
if (running) {
throw new LockedException();
}
//noinspection unchecked
this.measurements = (Collection<StandardDeviationFrameBodyMagneticFluxDensity>) measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public MagnetometerCalibratorMeasurementType getMeasurementType() {
return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_FRAME_BODY_MAGNETIC_FLUX_DENSITY;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer.
* When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
*
* @return true if z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer, false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mm matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for
* accelerometer, gyroscope and magnetometer, false
* otherwise.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this calibrator.
*
* @return listener to handle events raised by this calibrator.
*/
@Override
public KnownFrameMagnetometerNonLinearLeastSquaresCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2506 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2475 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2502 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2476 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1184 |
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @return collection of sequences of timestamped body kinematics
* measurements.
*/
@Override
public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3556 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2476 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3515 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2506 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2401 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2475 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2899 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2502 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2476 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1184 |
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
}
/**
* Gets collection of sequences of timestamped body kinematics
* measurements taken at a given position where the device moves freely
* with different orientations.
*
* @return collection of sequences of timestamped body kinematics
* measurements.
*/
@Override
public List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> getSequences() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2506 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2401 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2475 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3556 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2899 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2502 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2476 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3515 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1184 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3567 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3603 |
initialBiasZ = convertAngularSpeed(initialBias.getValueZ(), initialBias.getUnit());
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @return initial gyroscope scale factors and cross coupling errors
* matrix.
*/
@Override
public Matrix getInitialMg() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMg(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope scale factors and cross coupling errors
* matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial gyroscope scale factors and cross coupling errors matrix.
*
* @param initialMg initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMg(final Matrix initialMg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMg.getRows() != BodyKinematics.COMPONENTS || initialMg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMg.getElementAtIndex(0);
initialMyx = initialMg.getElementAtIndex(1);
initialMzx = initialMg.getElementAtIndex(2);
initialMxy = initialMg.getElementAtIndex(3);
initialSy = initialMg.getElementAtIndex(4);
initialMzy = initialMg.getElementAtIndex(5);
initialMxz = initialMg.getElementAtIndex(6);
initialMyz = initialMg.getElementAtIndex(7);
initialSz = initialMg.getElementAtIndex(8);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing initial g-dependent cross biases.
*/
@Override
public Matrix getInitialGg() {
return new Matrix(initialGg);
}
/**
* Gets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialGg(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.copyFrom(initialGg);
}
/**
* Sets initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @param initialGg g-dependent cross biases.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void setInitialGg(final Matrix initialGg) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialGg.getRows() != BodyKinematics.COMPONENTS || initialGg.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialGg.copyTo(this.initialGg);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1863 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1360 |
public void setListener(final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1927 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1421 |
public void setListener(final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurementsOrSequences() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1151 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 679 |
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Internal method to perform calibration when common z-axis is assumed for both
* the accelerometer and gyroscope.
*
* @throws AlgebraException if there are numerical errors.
*/
private void calibrateCommonAxis() throws AlgebraException {
// The accelerometer model is:
// fmeas = ba + (I + Ma) * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// fmeas = ba + (I + Ma) * ftrue
// Hence:
// [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
// [fmeasy] = [by] [0 1 0] [myx sy myz] [ftruey]
// [fmeasz] = [bz] [0 0 1] [mzx mzy sz ] [ftruez]
// where myx = mzx = mzy = 0
// Hence:
// [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
// [fmeasy] = [by] [0 1 0] [0 sy myz] [ftruey]
// [fmeasz] = [bz] [0 0 1] [0 0 sz ] [ftruez]
// [fmeasx] = [bx] + [1+sx mxy mxz ][ftruex]
// [fmeasy] [by] [0 1+sy myz ][ftruey]
// [fmeasz] [bz] [0 0 1+sz][ftruez]
// fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + (1+sy) * ftruey + myz * ftruez
// fmeasz = bz + (1+sz) * ftruez
// Where the unknowns are: sx, sy, sz, mxy mxz, myz
// Reordering:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + ftruez + sz * ftruez
// fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy - ftruey - by = sy * ftruey + myz * ftruez
// fmeasz - ftruez - bz = sz * ftruez
// [ftruex 0 0 ftruey ftruez 0 ][sx ] = [fmeasx - ftruex - bx]
// [0 ftruey 0 0 0 ftruez][sy ] [fmeasy - ftruey - by]
// [0 0 ftruez 0 0 0 ][sz ] [fmeasz - ftruez - bz]
// [mxy]
// [mxz]
// [myz]
final var expectedKinematics = new BodyKinematics();
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var fMeasX = measuredKinematics.getFx();
final var fMeasY = measuredKinematics.getFy();
final var fMeasZ = measuredKinematics.getFz();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, fTrueX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1173 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1137 |
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets a list of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() {
return measurements;
}
/**
* Sets a list of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @param measurements collection of body magnetic flux density measurements
* taken at different frames (positions, orientations
* and velocities).
* @throws LockedException if estimator is currently running.
*/
@Override
public void setMeasurements(
final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public MagnetometerCalibratorMeasurementType getMeasurementType() {
return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_FRAME_BODY_MAGNETIC_FLUX_DENSITY;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer.
* When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
*
* @return true if z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer, false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mm matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for
* accelerometer, gyroscope and magnetometer, false
* otherwise.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this calibrator.
*
* @return listener to handle events raised by this calibrator.
*/
public RobustKnownFrameMagnetometerCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1959 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1987 |
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public List<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
return measurements;
}
/**
* Sets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @param measurements collection of body magnetic flux density
* measurements at a known position and timestamp
* with unknown orientations.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final List<StandardDeviationBodyMagneticFluxDensity> measurements)
throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public MagnetometerCalibratorMeasurementType getMeasurementType() {
return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer.
* When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
*
* @return true if z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer, false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mm matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for
* accelerometer, gyroscope and magnetometer, false
* otherwise.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this calibrator.
*
* @return listener to handle events raised by this calibrator.
*/
public RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2101 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2060 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2794 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4055 |
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 2296 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1308 |
estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity,
position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final Point3D position, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final Point3D position, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static BodyKinematics estimateKinematicsAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz,
final double x, final double y, final double z) {
final var result = new BodyKinematics();
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
return result;
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between.
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static BodyKinematics estimateKinematicsAndReturnNew(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz,
final double x, final double y, final double z) {
final var result = new BodyKinematics();
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
return result;
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param velocity velocity of body frame with respect ECEF frame.
* @param oldVelocity previous velocity of body frame with respect ECEF frame.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static BodyKinematics estimateKinematicsAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1671 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1167 |
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets a list of body kinematics measurements taken at different
* frames (positions, orientations and velocities) and containing the standard
* deviations of accelerometer and gyroscope measurements.
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public List<StandardDeviationFrameBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a list of body kinematics measurements taken at different
* frames (positions, orientations and velocities) and containing the standard
* deviations of accelerometer and gyroscope measurements.
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @param measurements collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final List<StandardDeviationFrameBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public AccelerometerCalibratorMeasurementType getMeasurementType() {
return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_FRAME_BODY_KINEMATICS;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public RobustKnownBiasAndFrameAccelerometerCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1005 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1117 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1073 |
internalSetBias(bias);
}
/**
* Gets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return x coordinate of accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasX x coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return y coordinate of accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasY y coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return z coordinate of accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasZ z coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x coordinate of accelerometer bias.
*
* @return x coordinate of accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() {
return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known x coordinate of accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasXAsAcceleration(final Acceleration result) {
result.setValue(biasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known x coordinate of accelerometer bias.
*
* @param biasX x coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final Acceleration biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
}
/**
* Gets known y coordinate of accelerometer bias.
*
* @return y coordinate of accelerometer bias.
*/
@Override
public Acceleration getBiasYAsAcceleration() {
return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known y coordinate of accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasYAsAcceleration(final Acceleration result) {
result.setValue(biasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known y coordinate of accelerometer bias.
*
* @param biasY y coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final Acceleration biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAcceleration(biasY);
}
/**
* Gets known z coordinate of accelerometer bias.
*
* @return z coordinate of accelerometer bias.
*/
@Override
public Acceleration getBiasZAsAcceleration() {
return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets known z coordinate of accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasZAsAcceleration(final Acceleration result) {
result.setValue(biasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known z coordinate of accelerometer bias.
*
* @param biasZ z coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAcceleration(biasZ);
}
/**
* Sets known accelerometer bias coordinates expressed in meters per squared
* second (m/s^2).
*
* @param biasX x coordinate of accelerometer bias.
* @param biasY y coordinate of accelerometer bias.
* @param biasZ z coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 1819 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 2900 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1031 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 1823 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 2864 |
accelerometerSz = accelerometerMa.getElementAtIndex(8);
}
/**
* Gets x-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @return x-coordinate of gyroscope known bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets x-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @param biasX x-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets y-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @return y-coordinate of gyroscope known bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets y-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @param biasY y-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets z-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @return z-coordinate of gyroscope known bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets z-coordinate of gyroscope known bias.
* This is expressed in radians per second (rad/s).
*
* @param biasZ z-coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets x-coordinate of gyroscope known bias.
*
* @return x-coordinate of gyroscope known bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedX() {
return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets x-coordinate of gyroscope known bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(biasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets x-coordinate of gyroscope known bias.
*
* @param biasX x-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final AngularSpeed biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
}
/**
* Gets y-coordinate of gyroscope known bias.
*
* @return y-coordinate of gyroscope known bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedY() {
return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets y-coordinate of gyroscope known bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(biasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets y-coordinate of gyroscope known bias.
*
* @param biasY y-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final AngularSpeed biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAngularSpeed(biasY);
}
/**
* Gets z-coordinate of gyroscope known bias.
*
* @return initial z-coordinate of gyroscope known bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedZ() {
return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets z-coordinate of gyroscope known bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(biasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets z-coordinate of gyroscope known bias.
*
* @param biasZ z-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Sets known bias coordinates of gyroscope expressed in
* radians per second (rad/s).
*
* @param biasX x-coordinate of gyroscope known bias.
* @param biasY y-coordinate of gyroscope known bias.
* @param biasZ z-coordinate of gyroscope known bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(
final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 3758 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 709 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 1824 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1006 |
}
/**
* Gets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() {
return new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasXAsAcceleration(final Acceleration result) {
result.setValue(biasX);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets x-coordinate of known accelerometer bias.
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final Acceleration biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAcceleration(biasX);
}
/**
* Gets y-coordinate of known accelerometer bias.
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasYAsAcceleration() {
return new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets y-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasYAsAcceleration(final Acceleration result) {
result.setValue(biasY);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets y-coordinate of known accelerometer bias.
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final Acceleration biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAcceleration(biasY);
}
/**
* Gets z-coordinate of known accelerometer bias.
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasZAsAcceleration() {
return new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets z-coordinate of known accelerometer bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasZAsAcceleration(final Acceleration result) {
result.setValue(biasZ);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets z-coordinate of known accelerometer bias to be used to find a solution.
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final Acceleration biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAcceleration(biasZ);
}
/**
* Sets known bias coordinates of accelerometer expressed in meters
* per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @param biasY y-coordinate of known accelerometer bias.
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1453 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2804 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4065 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2111 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2070 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1514 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 718 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2658 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1032 |
}
/**
* Gets known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return x coordinate of gyroscope bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasX x coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return y coordinate of gyroscope bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasY y coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @return z coordinate of gyroscope bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z coordinate of gyroscope bias expressed in radians per second
* (rad/s).
*
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x coordinate of gyroscope bias.
*
* @return x coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedX() {
return new AngularSpeed(biasX, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known x coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedX(final AngularSpeed result) {
result.setValue(biasX);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known x coordinate of gyroscope bias.
*
* @param biasX x coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final AngularSpeed biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = convertAngularSpeed(biasX);
}
/**
* Gets known y coordinate of gyroscope bias.
*
* @return y coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedY() {
return new AngularSpeed(biasY, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known y coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedY(final AngularSpeed result) {
result.setValue(biasY);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known y coordinate of gyroscope bias.
*
* @param biasY y coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final AngularSpeed biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = convertAngularSpeed(biasY);
}
/**
* Gets known z coordinate of gyroscope bias.
*
* @return z coordinate of gyroscope bias.
*/
@Override
public AngularSpeed getBiasAngularSpeedZ() {
return new AngularSpeed(biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets known z coordinate of gyroscope bias.
*
* @param result instance where result data will be stored.
*/
@Override
public void getBiasAngularSpeedZ(final AngularSpeed result) {
result.setValue(biasZ);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known z coordinate of gyroscope bias.
*
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final AngularSpeed biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Sets known gyroscope bias coordinates expressed in radians per second
* (rad/s).
*
* @param biasX x coordinate of gyroscope bias.
* @param biasY y coordinate of gyroscope bias.
* @param biasZ z coordinate of gyroscope bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasCoordinates(final double biasX, final double biasY, final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2245 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2273 |
public void setListener(final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
}
/**
* Indicates whether calibrator is ready to start the estimator.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= getMinimumRequiredMeasurements()
&& position != null && year != null;
}
/**
* Indicates whether calibrator is currently running or no.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Gets Earth's magnetic model.
*
* @return Earth's magnetic model or null if not provided.
*/
public WorldMagneticModel getMagneticModel() {
return magneticModel;
}
/**
* Sets Earth's magnetic model.
*
* @param magneticModel Earth's magnetic model to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
if (running) {
throw new LockedException();
}
this.magneticModel = magneticModel;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 335 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 339 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 338 |
final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownFrameAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1215 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1185 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sequence.
* The larger the score value the better the quality of the sequence.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sequence.
* The larger the score value the better the quality of the sequence.
*
* @param qualityScores quality scores corresponding to each sequence.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return sequences.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(sequences.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustEasyGyroscopeCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1571 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1593 |
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
gravityNorm = computeGravityNorm();
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3396 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3459 |
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates gyroscope calibration parameters containing scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownBiasTurntableGyroscopeCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1940 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1944 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1507 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 342 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 336 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1511 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 339 |
super(measurements, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownFrameAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1878 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1941 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1878 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1435 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1374 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1941 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 336 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1511 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 339 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1941 |
super(measurements, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownFrameAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1690 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3396 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3459 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1940 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1944 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10367 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 10862 |
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener, DEFAULT_ROBUST_METHOD);
}
/**
* Computes error of a preliminary result respect a given measurement.
*
* @param measurement a measurement.
* @param preliminaryResult a preliminary result.
* @return computed error.
*/
protected double computeError(
final StandardDeviationBodyKinematics measurement, final PreliminaryResult preliminaryResult) {
// We know that measured angular rate is:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
// Hence:
// [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1 0] [myx sy myz] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1] [mzx mzy sz ] [Ωtruez] [g31 g32 g33][ftruez]
final var measuredKinematics = measurement.getKinematics();
final var specificForce = new double[]{
measuredKinematics.getFx(),
measuredKinematics.getFy(),
measuredKinematics.getFz()
};
try {
final var axis1 = ArrayUtils.normalizeAndReturnNew(specificForce);
final var rot1 = new Quaternion(axis1, 0.0);
final var nedC1 = new CoordinateTransformation(
rot1.asInhomogeneousMatrix(), FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
final var nedPosition = getNedPosition();
final var nedFrame1 = new NEDFrame(nedPosition, nedC1);
final var ecefFrame1 = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame1);
var ti = this.timeInterval;
var angleIncrement = turntableRotationRate * ti;
if (Math.abs(angleIncrement) > Math.PI / 2.0) {
// angle = rot_rate * interval
// rot_rate * interval / x = angle / x
// if we want angle / x = pi / 2, then:
final var x = Math.abs(angleIncrement) / (Math.PI / 2.0);
ti /= x;
angleIncrement = turntableRotationRate * ti;
}
final var rot = new AxisRotation3D(axis1, angleIncrement);
final var rot2 = rot1.combineAndReturnNew(rot);
final var nedC2 = new CoordinateTransformation(rot2.asInhomogeneousMatrix(), FrameType.BODY_FRAME,
FrameType.LOCAL_NAVIGATION_FRAME);
final var nedFrame2 = new NEDFrame(nedPosition, nedC2);
final var ecefFrame2 = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame2);
final var expectedKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(ti,
ecefFrame2, ecefFrame1);
final var angularRateMeasX1 = measuredKinematics.getAngularRateX();
final var angularRateMeasY1 = measuredKinematics.getAngularRateY();
final var angularRateMeasZ1 = measuredKinematics.getAngularRateZ();
final var angularRateTrueX = expectedKinematics.getAngularRateX();
final var angularRateTrueY = expectedKinematics.getAngularRateY();
final var angularRateTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
final var mg = preliminaryResult.estimatedMg; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4743 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4037 |
for (final var sequence : sequences) {
fixedSequences.add(new BodyKinematicsSequence<>(sequence));
}
accelerationFixer.setBias(ba);
accelerationFixer.setCrossCouplingErrors(ma);
var i = 0;
for (final var sequence : sequences) {
// sequence mean accelerometer samples of previous static
// period will need to be fixed using accelerometer calibration
// parameters
measuredBeforeF[0] = sequence.getBeforeMeanFx();
measuredBeforeF[1] = sequence.getBeforeMeanFy();
measuredBeforeF[2] = sequence.getBeforeMeanFz();
accelerationFixer.fix(measuredBeforeF, fixedBeforeF);
measuredAfterF[0] = sequence.getAfterMeanFx();
measuredAfterF[1] = sequence.getAfterMeanFy();
measuredAfterF[2] = sequence.getAfterMeanFz();
accelerationFixer.fix(measuredAfterF, fixedAfterF);
// because we are only interested in gravity direction, we
// normalize these vectors, so that gravity becomes independent
// of current Earth position.
ArrayUtils.normalize(fixedBeforeF);
ArrayUtils.normalize(fixedAfterF);
x.setSubmatrix(i, 0, i, 2, fixedBeforeF);
x.setSubmatrix(i, 3, i, 5, fixedAfterF);
y[i] = 0.0;
standardDeviations[i] = computeAverageAngularRateStandardDeviation(sequence);
i++;
}
fitter.setInputData(x, y, standardDeviations);
}
/**
* Computes average angular rate standard deviation for measurements
* in provided sequence.
*
* @param sequence a sequence.
* @return average angular rate standard deviation expressed in radians
* per second (rad/s).
*/
private static double computeAverageAngularRateStandardDeviation(
final BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> sequence) {
final var items = sequence.getSortedItems();
final var size = items.size();
var result = 0.0;
for (final var item : items) {
result += item.getAngularRateStandardDeviation() / size;
}
return result;
}
/**
* Converts acceleration instance to meters per squared second.
*
* @param acceleration acceleration instance to be converted.
* @return converted value.
*/
private static double convertAcceleration(final Acceleration acceleration) {
return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Converts angular speed value and unit to radians per second.
*
* @param value angular speed value.
* @param unit unit of angular speed value.
* @return converted value.
*/
private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Converts angular speed instance to radians per second.
*
* @param angularSpeed angular speed instance to be converted.
* @return converted value.
*/
private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
}
/**
* Makes proper conversion of internal cross-coupling, bias and g-dependent
* cross bias matrices.
*
* @param m internal scaling and cross-coupling matrix.
* @param b internal bias matrix.
* @param g internal g-dependent cross bias matrix.
* @throws AlgebraException if a numerical instability occurs.
*/
private void setResult(final Matrix m, final Matrix b, final Matrix g) throws AlgebraException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2028 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2073 |
public void setListener(final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= getMinimumRequiredMeasurements()
&& groundTruthGravityNorm != null;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1987 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2034 |
public void setListener(final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= getMinimumRequiredMeasurements() && position != null;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2133 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2161 |
public void setListener(final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
}
/**
* Indicates whether calibrator is ready to start the estimator.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= getMinimumRequiredMeasurements()
&& groundTruthMagneticFluxDensityNorm != null;
}
/**
* Indicates whether calibrator is currently running or no.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1396 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1457 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1422 |
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1900 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1963 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1386 |
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5871 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3007 |
setResult(crossCoupling, bias);
// at this point covariance is expressed in terms of b and M, and must
// be expressed in terms of ba and Ma.
// We know that:
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [m21 m22 m23]
// [m31 m32 m33]
// and that ba and Ma are expressed as:
// Ma = M - I
// ba = M * b
// Ma = [m11 - 1 m12 m13 ] = [sx mxy mxz]
// [m21 m22 - 1 m23 ] [myx sy myz]
// [m31 m32 m33 - 1] [mzx mzy sz ]
// ba = [m11 * bx + m12 * by + m13 * bz] = [bax]
// [m21 * bx + m22 * by + m23 * bz] [bay]
// [m31 * bx + m32 * by + m33 * bz] [baz]
// Defining the linear application:
// F(b, M) = F(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33)
// as:
// [bax] = [m11 * bx + m12 * by + m13 * bz]
// [bay] [m21 * bx + m22 * by + m23 * bz]
// [baz] [m31 * bx + m32 * by + m33 * bz]
// [sx] [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [m21]
// [myz] [m23]
// [mzx] [m31]
// [mzy] [m32]
// Then the Jacobian of F(b, M) is:
// J = [m11 m12 m13 bx 0 0 by 0 0 bz 0 0 ]
// [m21 m22 m23 0 bx 0 0 by 0 0 bz 0 ]
// [m31 m32 m33 0 0 bx 0 0 by 0 0 bz]
// [0 0 0 1 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 1 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 1 ]
// [0 0 0 0 0 0 1 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 1 0 0 ]
// [0 0 0 0 1 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 1 0 ]
// [0 0 0 0 0 1 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 1 0 0 0 ]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS, GENERAL_UNKNOWNS);
jacobian.setElementAt(0, 0, m11);
jacobian.setElementAt(1, 0, m21);
jacobian.setElementAt(2, 0, m31);
jacobian.setElementAt(0, 1, m12);
jacobian.setElementAt(1, 1, m22);
jacobian.setElementAt(2, 1, m32);
jacobian.setElementAt(0, 2, m13);
jacobian.setElementAt(1, 2, m23);
jacobian.setElementAt(2, 2, m33);
jacobian.setElementAt(0, 3, bx);
jacobian.setElementAt(3, 3, 1.0);
jacobian.setElementAt(1, 4, bx);
jacobian.setElementAt(8, 4, 1.0);
jacobian.setElementAt(2, 5, bx);
jacobian.setElementAt(10, 5, 1.0);
jacobian.setElementAt(0, 6, by);
jacobian.setElementAt(6, 6, 1.0);
jacobian.setElementAt(1, 7, by);
jacobian.setElementAt(4, 7, 1.0);
jacobian.setElementAt(2, 8, by);
jacobian.setElementAt(11, 8, 1.0);
jacobian.setElementAt(0, 9, bz);
jacobian.setElementAt(7, 9, 1.0);
jacobian.setElementAt(1, 10, bz);
jacobian.setElementAt(9, 10, 1.0);
jacobian.setElementAt(2, 11, bz);
jacobian.setElementAt(5, 11, 1.0);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Internal method to perform calibration when common z-axis is assumed for both
* the accelerometer and gyroscope.
*
* @throws FittingException if Levenberg-Marquardt fails for numerical reasons.
* @throws AlgebraException if there are numerical instabilities that prevent
* matrix inversion.
* @throws com.irurueta.numerical.NotReadyException never happens.
*/
private void calibrateCommonAxis() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException {
// The accelerometer model is:
// fmeas = ba + (I + Ma) * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// fmeas = ba + (I + Ma) * ftrue
// For convergence purposes of the Levenberg-Marquardt algorithm, the
// accelerometer model can be better expressed as:
// fmeas = T*K*(ftrue + b)
// fmeas = M*(ftrue + b)
// fmeas = M*ftrue + M*b
// where:
// M = I + Ma
// ba = M*b = (I + Ma)*b --> b = M^-1*ba
// We know that the norm of the true specific force is equal to the amount
// of gravity at a certain Earth position
// ||ftrue|| = ||g|| ~ 9.81 m/s^2
// Hence:
// fmeas - M*b = M*ftrue
// M^-1 * (fmeas - M*b) = ftrue
// ||g||^2 = ||ftrue||^2 = (M^-1 * (fmeas - M*b))^T * (M^-1 * (fmeas - M*b))
// ||g||^2 = (fmeas - M*b)^T*(M^-1)^T * M^-1 * (fmeas - M*b)
// ||g||^2 = (fmeas - M * b)^T * ||M^-1||^2 * (fmeas - M * b)
// ||g||^2 = ||fmeas - M * b||^2 * ||M^-1||^2
// Where:
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [0 m22 m23]
// [0 0 m33]
final var gradientEstimator = new GradientEstimator(this::evaluateCommonAxis);
final var initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1900 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1963 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1422 |
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1396 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1457 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1386 |
}
/**
* Indicates whether a linear calibrator is used or not for preliminary
* solutions.
*
* @return indicates whether a linear calibrator is used or not for
* preliminary solutions.
*/
public boolean isLinearCalibratorUsed() {
return useLinearCalibrator;
}
/**
* Specifies whether a linear calibrator is used or not for preliminary
* solutions.
*
* @param linearCalibratorUsed indicates whether a linear calibrator is used
* or not for preliminary solutions.
* @throws LockedException if calibrator is currently running.
*/
public void setLinearCalibratorUsed(final boolean linearCalibratorUsed) throws LockedException {
if (running) {
throw new LockedException();
}
useLinearCalibrator = linearCalibratorUsed;
}
/**
* Indicates whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @return true if preliminary solutions must be refined after an initial linear solution, false
* otherwise.
*/
public boolean isPreliminarySolutionRefined() {
return refinePreliminarySolutions;
}
/**
* Specifies whether preliminary solutions must be refined after an initial linear solution is found.
* If no initial solution is found using a linear solver, a non linear solver will be
* used regardless of this value using an average solution as the initial value to be
* refined.
*
* @param preliminarySolutionRefined true if preliminary solutions must be refined after an
* initial linear solution, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
if (running) {
throw new LockedException();
}
refinePreliminarySolutions = preliminarySolutionRefined;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6247 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3384 |
estimatedMa.setElementAt(i, i, estimatedMa.getElementAt(i, i) - 1.0);
}
estimatedCovariance = fitter.getCovar();
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters for the general case.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param params array containing current parameters for the general purpose case.
* Must have length 12.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateGeneral(final double[] params) throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m21 = params[4];
final var m31 = params[5];
final var m12 = params[6];
final var m22 = params[7];
final var m32 = params[8];
final var m13 = params[9];
final var m23 = params[10];
final var m33 = params[11];
return evaluate(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33);
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters when common z-axis is assumed.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param params array containing current parameters for the common z-axis case.
* Must have length 9.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateCommonAxis(final double[] params) throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m12 = params[4];
final var m22 = params[5];
final var m13 = params[6];
final var m23 = params[7];
final var m33 = params[8];
return evaluate(bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33);
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param bx x-coordinate of bias.
* @param by y-coordinate of bias.
* @param bz z-coordinate of bias.
* @param m11 element 1,1 of cross-coupling error matrix.
* @param m21 element 2,1 of cross-coupling error matrix.
* @param m31 element 3,1 of cross-coupling error matrix.
* @param m12 element 1,2 of cross-coupling error matrix.
* @param m22 element 2,2 of cross-coupling error matrix.
* @param m32 element 3,2 of cross-coupling error matrix.
* @param m13 element 1,3 of cross-coupling error matrix.
* @param m23 element 2,3 of cross-coupling error matrix.
* @param m33 element 3,3 of cross-coupling error matrix.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluate(final double bx, final double by, final double bz,
final double m11, final double m21, final double m31,
final double m12, final double m22, final double m32,
final double m13, final double m23, final double m33) throws EvaluationException {
// fmeas = M*(ftrue + b)
// ftrue = M^-1*fmeas - b
try {
if (fmeas == null) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 973 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 977 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2917 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known accelerometer bias as a column matrix.
*
* @return known accelerometer bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known accelerometer bias as a column matrix.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4302 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 973 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2370 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1683 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1637 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 977 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2917 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2363 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3446 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3408 |
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known bias to be set
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known bias as a column matrix.
*
* @return known bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known bias as an array.
*
* @param bias bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 715 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 387 |
estimateKinematics(timeInterval, c, oldC, velocity, oldVelocity, position, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final Point3D position, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final Point3D position, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz,
final double x, final double y, final double z) {
return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between.
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz,
final double x, final double y, final double z) {
return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param velocity velocity of body frame with respect ECEF frame.
* @param oldVelocity previous velocity of body frame with respect ECEF frame.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1782 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3271 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1029 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1852 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1817 |
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getHardIron() {
final var result = new double[BodyMagneticFluxDensity.COMPONENTS];
getHardIron(result);
return result;
}
/**
* Gets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
@Override
public void getHardIron(final double[] result) {
if (result.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = hardIronX;
result[1] = hardIronY;
result[2] = hardIronZ;
}
/**
* Sets known hard-iron bias as an array.
* Array values are expressed in Teslas (T).
*
* @param hardIron known hard-iron.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setHardIron(final double[] hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.length != BodyMagneticFluxDensity.COMPONENTS) {
throw new IllegalArgumentException();
}
hardIronX = hardIron[0];
hardIronY = hardIron[1];
hardIronZ = hardIron[2];
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @return known hard-iron bias as a column matrix.
*/
@Override
public Matrix getHardIronMatrix() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
getHardIronMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known hard-iron bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getHardIronMatrix(final Matrix result) {
if (result.getRows() != BodyMagneticFluxDensity.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, hardIronX);
result.setElementAtIndex(1, hardIronY);
result.setElementAtIndex(2, hardIronZ);
}
/**
* Sets known hard-iron bias as a column matrix.
*
* @param hardIron known hard-iron bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setHardIron(final Matrix hardIron) throws LockedException {
if (running) {
throw new LockedException();
}
if (hardIron.getRows() != BodyMagneticFluxDensity.COMPONENTS || hardIron.getColumns() != 1) {
throw new IllegalArgumentException();
}
hardIronX = hardIron.getElementAtIndex(0);
hardIronY = hardIron.getElementAtIndex(1);
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2371 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3433 |
final var result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @return initial gyroscope bias to be used to find a solution as a
* column matrix.
*/
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* a column matrix.
* Values are expressed in radians per second (rad/s).
*
* @param initialBias initial gyroscope bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @return initial bias coordinates.
*/
public AngularSpeedTriad getInitialBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @param result instance where result will be stored.
*/
public void getInitialBiasAsTriad(AngularSpeedTriad result) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2368 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3471 |
final double[] result = new double[BodyKinematics.COMPONENTS];
getInitialBias(result);
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void getInitialBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = initialBiasX;
result[1] = initialBiasY;
result[2] = initialBiasZ;
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* an array.
* Array values are expressed in radians per second (rad/s).
*
* @param initialBias initial bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
public void setInitialBias(final double[] initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias[0];
initialBiasY = initialBias[1];
initialBiasZ = initialBias[2];
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @return initial gyroscope bias to be used to find a solution as a
* column matrix.
*/
public Matrix getInitialBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getInitialBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial gyroscope bias to be used to find a solution as a
* column matrix.
* Values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void getInitialBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialBiasX);
result.setElementAtIndex(1, initialBiasY);
result.setElementAtIndex(2, initialBiasZ);
}
/**
* Sets initial gyroscope bias to be used to find a solution as
* a column matrix with values expressed in radians per second (rad/s).
*
* @param initialBias initial gyroscope bias to find a solution.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void setInitialBias(final Matrix initialBias) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialBias.getRows() != BodyKinematics.COMPONENTS || initialBias.getColumns() != 1) {
throw new IllegalArgumentException();
}
initialBiasX = initialBias.getElementAtIndex(0);
initialBiasY = initialBias.getElementAtIndex(1);
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @return initial bias coordinates.
*/
public AngularSpeedTriad getInitialBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, initialBiasX, initialBiasY, initialBiasZ);
}
/**
* Gets initial bias coordinates of gyroscope used to find a solution.
*
* @param result instance where result will be stored.
*/
public void getInitialBiasAsTriad(AngularSpeedTriad result) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1127 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2211 |
fillHardIronBiases(bx, by, bz);
fillMm(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
}
/**
* Internal method to perform general calibration.
*
* @throws AlgebraException if there are numerical errors.
* @throws IOException if world magnetic model cannot be loaded.
*/
private void calibrateGeneral() throws AlgebraException, IOException {
// The magnetometer model is:
// mBmeas = bm + (I + Mm) * mBtrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// mBmeas = bm + (I + Mm) * mBtrue
// Hence:
// [mBmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [mBtruex]
// [mBmeasy] = [by] [0 1 0] [myx sy myz] [mBtruey]
// [mBmeasz] = [bz] [0 0 1] [mzx mzy sz ] [mBtruez]
// [mBmeasx] = [bx] + [1+sx mxy mxz ][mBtruex]
// [mBmeasy] [by] [myx 1+sy myz ][mBtruey]
// [mBmeasz] [bz] [mzx mzy 1+sz][mBtruez]
// mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy = by + myx * mBtruex + (1+sy) * mBtruey + myz * mBtruez
// mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + (1+sz) * mBtruez
// Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
// Reordering:
// mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy = by + myx * mBtruex + mBtruey + sy * mBtruey + myz * mBtruez
// mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + mBtruez + sz * mBtruez
// mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy - mBtruey = by + myx * mBtruex + sy * mBtruey + myz * mBtruez
// mBmeasz - mBtruez = bz + mzx * mBtruex + mzy * mBtruey + sz * mBtruez
// [1 0 0 mBtruex 0 0 mBtruey mBtruez 0 0 0 0 ][bx ] = [mBmeasx - mBtruex]
// [0 1 0 0 mBtruey 0 0 0 mBtruex mBtruez 0 0 ][by ] [mBmeasy - mBtruey]
// [0 0 1 0 0 mBtruez 0 0 0 0 mBtruex mBtruey][bz ] [mBmeasz - mBtruez]
// [sx ]
// [sy ]
// [sz ]
// [mxy]
// [mxz]
// [myx]
// [myz]
// [mzx]
// [mzy]
final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
if (magneticModel != null) {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
} else {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
}
final var expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
final var nedFrame = new NEDFrame();
final var earthB = new NEDMagneticFluxDensity();
final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
// estimate Earth magnetic flux density at frame position and
// timestamp using WMM
final var ecefFrame = measurement.getFrame();
ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);
final var year = measurement.getYear();
final var latitude = nedFrame.getLatitude();
final var longitude = nedFrame.getLongitude();
final var height = nedFrame.getHeight();
nedFrame.getCoordinateTransformation(cbn);
cbn.inverse(cnb);
wmmEstimator.estimate(latitude, longitude, height, year, earthB);
// estimate expected body magnetic flux density taking into
// account body attitude (inverse of frame orientation) and
// estimated Earth magnetic flux density
BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);
final var bMeasX = measuredMagneticFluxDensity.getBx();
final var bMeasY = measuredMagneticFluxDensity.getBy();
final var bMeasZ = measuredMagneticFluxDensity.getBz();
final var bTrueX = expectedMagneticFluxDensity.getBx();
final var bTrueY = expectedMagneticFluxDensity.getBy();
final var bTrueZ = expectedMagneticFluxDensity.getBz();
a.setElementAt(i, 0, 1.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java | 1219 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 1189 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == sequences.size();
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(
new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return sequences.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(sequences.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustEasyGyroscopeCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 343 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 343 |
final boolean commonAxisUsed, final RobustKnownFrameGyroscopeCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(
new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownFrameGyroscopeCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1576 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1594 |
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
gravityNorm = computeGravityNorm();
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3891 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5240 |
return evaluateGeneral(i, params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var m11 = result[0];
final var m21 = result[1];
final var m31 = result[2];
final var m12 = result[3];
final var m22 = result[4];
final var m32 = result[5];
final var m13 = result[6];
final var m23 = result[7];
final var m33 = result[8];
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mm.setElementAtIndex(0, m11);
mm.setElementAtIndex(1, m21);
mm.setElementAtIndex(2, m31);
mm.setElementAtIndex(3, m12);
mm.setElementAtIndex(4, m22);
mm.setElementAtIndex(5, m32);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33);
setResult(mm);
// at this point covariance is expressed in terms of M, and must
// be expressed in terms of Mg.
// We know that:
// Mg = M - I
// Gg = M * G
// M = [m11 m12 m13]
// [m21 m22 m23]
// [m31 m32 m33]
// G = [g11 g12 g13] = 0
// [g21 g22 g23]
// [g31 g32 g33]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [m21 m22 - 1 m23 ]
// [mzx mzy sz ] [m31 m32 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = [m11 m12 m13][g11 g12 g13]
// [gg21 gg22 gg23] [m21 m22 m23][g21 g22 g23]
// [gg31 gg32 gg33] [m31 m32 m33][g31 g32 g33]
// Defining the linear application:
// F(M) = F(m11, m21, m31, m12, m22, m32, m13, m23, m33)
// as:
// [sx] = [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [m21]
// [myz] [m23]
// [mzx] [m31]
// [mzy] [m32]
// [gg11] [0]
// [gg21] [0]
// [gg31] [0]
// [gg12] [0]
// [gg22] [0]
// [gg32] [0]
// [gg13] [0]
// [gg23] [0]
// [gg33] [0]
// Then the Jacobian of F(M) is:
// J = [1 0 0 0 0 0 0 0 0]
// [0 0 0 0 1 0 0 0 0]
// [0 0 0 0 0 0 0 0 1]
// [0 0 0 1 0 0 0 0 0]
// [0 0 0 0 0 0 1 0 0]
// [0 1 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 1 0]
// [0 0 1 0 0 0 0 0 0]
// [0 0 0 0 0 1 0 0 0]
// [0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, GENERAL_UNKNOWNS);
jacobian.setElementAt(0, 0, 1.0);
jacobian.setElementAt(1, 4, 1.0);
jacobian.setElementAt(2, 8, 1.0);
jacobian.setElementAt(3, 3, 1.0);
jacobian.setElementAt(4, 6, 1.0);
jacobian.setElementAt(5, 1, 1.0);
jacobian.setElementAt(6, 7, 1.0);
jacobian.setElementAt(7, 2, 1.0);
jacobian.setElementAt(8, 5, 1.9);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Sets input data into Levenberg-Marquardt fitter.
*
* @throws AlgebraException if there are numerical instabilities.
*/
private void setInputData() throws AlgebraException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2781 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2754 |
return EasyGyroscopeCalibrator.MINIMUM_SEQUENCES_GENERAL;
}
}
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3990 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4042 |
return KnownBiasTurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_GENERAL;
}
}
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= getMinimumRequiredMeasurementsOrSequences();
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1577 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 1149 |
}
/**
* Fills scale factor and cross coupling error matrix with estimated values.
*
* @param sx x scale factor
* @param sy y scale factor
* @param sz z scale factor
* @param mxy x-y cross coupling
* @param mxz x-z cross coupling
* @param myx y-x cross coupling
* @param myz y-z cross coupling
* @param mzx z-x cross coupling
* @param mzy z-y cross coupling
* @throws WrongSizeException never happens.
*/
private void fillMg(final double sx, final double sy, final double sz,
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy) throws WrongSizeException {
if (estimatedMg == null) {
estimatedMg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
estimatedMg.setElementAt(0, 0, sx);
estimatedMg.setElementAt(1, 0, myx);
estimatedMg.setElementAt(2, 0, mzx);
estimatedMg.setElementAt(0, 1, mxy);
estimatedMg.setElementAt(1, 1, sy);
estimatedMg.setElementAt(2, 1, mzy);
estimatedMg.setElementAt(0, 2, mxz);
estimatedMg.setElementAt(1, 2, myz);
estimatedMg.setElementAt(2, 2, sz);
}
/**
* Fills G-dependant cross biases.
*
* @param g11 element 1,1 of G-dependant cross biases matrix.
* @param g12 element 1,2 of G-dependant cross biases matrix.
* @param g13 element 1,3 of G-dependant cross biases matrix.
* @param g21 element 2,1 of G-dependant cross biases matrix.
* @param g22 element 2,2 of G-dependant cross biases matrix.
* @param g23 element 2,3 of G-dependant cross biases matrix.
* @param g31 element 3,1 of G-dependant cross biases matrix.
* @param g32 element 3,2 of G-dependant cross biases matrix.
* @param g33 element 3,3 of G-dependant cross biases matrix.
* @throws WrongSizeException never happens.
*/
private void fillGg(final double g11, final double g12, final double g13,
final double g21, final double g22, final double g23,
final double g31, final double g32, final double g33) throws WrongSizeException {
if (estimatedGg == null) {
estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
estimatedGg.setElementAt(0, 0, g11);
estimatedGg.setElementAt(0, 1, g12);
estimatedGg.setElementAt(0, 2, g13);
estimatedGg.setElementAt(1, 0, g21);
estimatedGg.setElementAt(1, 1, g22);
estimatedGg.setElementAt(1, 2, g23);
estimatedGg.setElementAt(2, 0, g31);
estimatedGg.setElementAt(2, 1, g32);
estimatedGg.setElementAt(2, 2, g33);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4595 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5977 |
setResult(m, b);
// at this point covariance is expressed in terms of b, M and G, and must
// be expressed in terms of bg, Mg and Gg.
// We know that:
// bg = M * b
// Mg = M - I
// Gg = M * G = 0
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [m21 m22 m23]
// [m31 m32 m33]
// G = [g11 g12 g13] = 0
// [g21 g22 g23]
// [g31 g32 g33]
// bg = [m11 m12 m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
// [m21 m22 m23][by] [m21 * bx + m22 * by + m23 * bz] [bgy]
// [m31 m32 m33][bz] [m31 * bx + m32 * by + m33 * bz] [bgz]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [m21 m22 - 1 m23 ]
// [mzx mzy sz ] [m31 m32 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = 0
// [gg21 gg22 gg23]
// [gg31 gg32 gg33]
// Defining the linear application:
// F(b, M) = F(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33)
// as:
// [bgx] = [m11 * bx + m12 * by + m13 * bz]
// [bgy] [m21 * bx + m22 * by + m23 * bz]
// [bgz] [m31 * bx + m32 * by + m33 * bz]
// [sx] [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [m21]
// [myz] [m23]
// [mzx] [m31]
// [mzy] [m32]
// [gg11] [0]
// [gg21] [0]
// [gg31] [0]
// [gg12] [0]
// [gg22] [0]
// [gg32] [0]
// [gg13] [0]
// [gg23] [0]
// [gg33] [0]
// Then the Jacobian of F(b, M) is:
// J = [m11 m12 m13 bx 0 0 by 0 0 bz 0 0 ]
// [m21 m22 m23 0 bx 0 0 by 0 0 bz 0 ]
// [m31 m32 m33 0 0 bx 0 0 by 0 0 bz]
// [0 0 0 1 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 1 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 1 ]
// [0 0 0 0 0 0 1 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 1 0 0 ]
// [0 0 0 0 1 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 1 0 ]
// [0 0 0 0 0 1 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 1 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 ]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, GENERAL_UNKNOWNS);
jacobian.setElementAt(0, 0, m11);
jacobian.setElementAt(0, 1, m12);
jacobian.setElementAt(0, 2, m13);
jacobian.setElementAt(0, 3, bx);
jacobian.setElementAt(0, 6, by);
jacobian.setElementAt(0, 9, bz);
jacobian.setElementAt(1, 0, m21);
jacobian.setElementAt(1, 1, m22);
jacobian.setElementAt(1, 2, m23);
jacobian.setElementAt(1, 4, bx);
jacobian.setElementAt(1, 7, by);
jacobian.setElementAt(1, 10, bz);
jacobian.setElementAt(2, 0, m31);
jacobian.setElementAt(2, 1, m32);
jacobian.setElementAt(2, 2, m33);
jacobian.setElementAt(2, 5, bx);
jacobian.setElementAt(2, 8, by);
jacobian.setElementAt(2, 11, bz);
jacobian.setElementAt(3, 3, 1.0);
jacobian.setElementAt(4, 7, 1.0);
jacobian.setElementAt(5, 11, 1.0);
jacobian.setElementAt(6, 6, 1.0);
jacobian.setElementAt(7, 9, 1.0);
jacobian.setElementAt(8, 4, 1.0);
jacobian.setElementAt(9, 10, 1.0);
jacobian.setElementAt(10, 5, 1.0);
jacobian.setElementAt(11, 8, 1.9);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Sets input data into Levenberg-Marquardt fitter.
*
* @throws AlgebraException if there are numerical instabilities.
*/
private void setInputData() throws AlgebraException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3399 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3464 |
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates gyroscope calibration parameters containing scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(
new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1943 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1948 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(
new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 2169 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1237 |
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
estimateKinematics(timeInterval, frame, oldC,
SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz, final Point3D position,
final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz, final Point3D position,
final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz,
position.getInhomX(), position.getInhomY(), position.getInhomZ(), result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param velocity velocity of body frame with respect ECEF frame.
* @param oldVelocity previous velocity of body frame with respect ECEF frame.
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1509 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 346 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1517 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 344 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 344 |
super(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(
new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1517 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 344 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 344 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1944 |
super(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(
new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2834 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4110 |
} catch (final FittingException | AlgebraException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Internal method to perform calibration when common z-axis is assumed
* for both the accelerometer and gyroscope and when G-dependent cross
* biases are being estimated.
*
* @throws AlgebraException if accelerometer parameters prevent fixing
* measured accelerometer values due to numerical instabilities.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateCommonAxisAndGDependentCrossBiases() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6252 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6423 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3389 |
estimatedMse = fitter.getMse();
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters for the general case.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param params array containing current parameters for the general purpose case.
* Must have length 12.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateGeneral(final double[] params) throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m21 = params[4];
final var m31 = params[5];
final var m12 = params[6];
final var m22 = params[7];
final var m32 = params[8];
final var m13 = params[9];
final var m23 = params[10];
final var m33 = params[11];
return evaluate(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33);
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters when common z-axis is assumed.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param params array containing current parameters for the common z-axis case.
* Must have length 9.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateCommonAxis(final double[] params) throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m12 = params[4];
final var m22 = params[5];
final var m13 = params[6];
final var m23 = params[7];
final var m33 = params[8];
return evaluate(bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33);
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param bx x-coordinate of bias.
* @param by y-coordinate of bias.
* @param bz z-coordinate of bias.
* @param m11 element 1,1 of cross-coupling error matrix.
* @param m21 element 2,1 of cross-coupling error matrix.
* @param m31 element 3,1 of cross-coupling error matrix.
* @param m12 element 1,2 of cross-coupling error matrix.
* @param m22 element 2,2 of cross-coupling error matrix.
* @param m32 element 3,2 of cross-coupling error matrix.
* @param m13 element 1,3 of cross-coupling error matrix.
* @param m23 element 2,3 of cross-coupling error matrix.
* @param m33 element 3,3 of cross-coupling error matrix.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluate(final double bx, final double by, final double bz,
final double m11, final double m21, final double m31,
final double m12, final double m22, final double m32,
final double m13, final double m23, final double m33) throws EvaluationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1676 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1695 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1676 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1695 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3399 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3464 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1943 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1948 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 184 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 186 |
final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3092 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2834 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated position.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Internal method to perform calibration when common z-axis is assumed for both
* the accelerometer and gyroscope.
*
* @throws AlgebraException if there are numerical errors.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateCommonAxis() throws AlgebraException, FittingException, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 596 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 580 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return sequences.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(sequences.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustEasyGyroscopeCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 900 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 905 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 759 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 768 |
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
gravityNorm = computeGravityNorm();
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4664 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2752 |
listener.onCalibrateEnd((C) this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5794 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 5316 |
angularRateFixer.setBias(preliminaryResult.estimatedBiases);
angularRateFixer.setCrossCouplingErrors(preliminaryResult.estimatedMg);
angularRateFixer.setGDependantCrossBias(preliminaryResult.estimatedGg);
// copy measured sequence as it will be used to fix kinematics values
// using preliminary gyroscope parameters
final var fixedSequence = new BodyKinematicsSequence<>(sequence);
// fix body kinematic measurements of provided sequence
final var numItems = sequence.getItemsCount();
final var measuredItems = sequence.getSortedItems();
final var fixedItems = fixedSequence.getSortedItems();
for (var j = 0; j < numItems; j++) {
final var measuredItem = measuredItems.get(j);
final var fixedItem = fixedItems.get(j);
final var measuredKinematics = measuredItem.getKinematics();
final var fixedKinematics = fixedItem.getKinematics();
fixKinematics(measuredKinematics, fixedKinematics);
}
// integrate fixed sequence to obtain attitude change
QuaternionIntegrator.integrateGyroSequence(fixedSequence, QuaternionStepIntegratorType.RUNGE_KUTTA, q);
// fix before coordinates
measuredSpecificForce[0] = sequence.getBeforeMeanFx();
measuredSpecificForce[1] = sequence.getBeforeMeanFy();
measuredSpecificForce[2] = sequence.getBeforeMeanFz();
accelerationFixer.fix(measuredSpecificForce, fixedSpecificForce);
// normalize coordinates
ArrayUtils.normalize(fixedSpecificForce);
// compute estimated normalized end coordinates
startPoint.setCoordinates(fixedSpecificForce);
q.inverse();
q.rotate(startPoint, endPoint);
// fix after coordinates
measuredSpecificForce[0] = sequence.getAfterMeanFx();
measuredSpecificForce[1] = sequence.getAfterMeanFy();
measuredSpecificForce[2] = sequence.getAfterMeanFz();
accelerationFixer.fix(measuredSpecificForce, fixedSpecificForce);
// normalize coordinates
ArrayUtils.normalize(fixedSpecificForce);
expectedEndPoint.setCoordinates(fixedSpecificForce);
// compare estimated normalized end coordinates with expected
// ones
return expectedEndPoint.distanceTo(endPoint);
} catch (final AlgebraException | RotationException e) {
return Double.MAX_VALUE;
}
}
/**
* Computes a preliminary solution for a subset of samples picked by a robust estimator.
*
* @param samplesIndices indices of samples picked by the robust estimator.
* @param solutions list where estimated preliminary solution will be stored.
*/
protected void computePreliminarySolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
final var seqs = new ArrayList<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>>();
for (final var samplesIndex : samplesIndices) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4409 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2699 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4743 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2477 |
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1797 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3417 |
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1879 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3379 |
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1173 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1959 |
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets a list of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1137 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1987 |
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets a list of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public List<StandardDeviationFrameBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java | 5217 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java | 5715 |
this(convertPosition(position), measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @return position where body kinematics measures have been taken.
*/
public ECEFPosition getEcefPosition() {
return position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final ECEFPosition position) throws LockedException {
if (isRunning()) {
throw new LockedException();
}
this.position = position;
if (position != null) {
final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
this.position.getX(), this.position.getY(), this.position.getZ());
groundTruthGravityNorm = gravity.getNorm();
} else {
groundTruthGravityNorm = null;
}
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @return position where body kinematics measures have been taken or null if
* not available.
*/
public NEDPosition getNedPosition() {
final var result = new NEDPosition();
return getNedPosition(result) ? result : null;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param result instance where result will be stored.
* @return true if NED position could be computed, false otherwise.
*/
public boolean getNedPosition(final NEDPosition result) {
if (position != null) {
final var velocity = new NEDVelocity();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
position.getX(), position.getY(), position.getZ(),
0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final NEDPosition position) throws LockedException {
setPosition(position != null ? convertPosition(position) : null);
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && position != null;
}
/**
* Converts provided NED position expressed in terms of latitude, longitude and height respect
* mean Earth surface, to position expressed in ECEF coordinates.
*
* @param position NED position to be converted.
* @return converted position expressed in ECEF coordinates.
*/
private static ECEFPosition convertPosition(final NEDPosition position) {
final var velocity = new ECEFVelocity();
final var result = new ECEFPosition();
NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
position.getLatitude(), position.getLongitude(), position.getHeight(),
0.0, 0.0, 0.0, result, velocity);
return result;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1641 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1673 |
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates gyroscope calibration parameters containing scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownBiasTurntableGyroscopeCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2793 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4002 |
return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2766 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4054 |
return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4409 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2477 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1167 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 704 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 189 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 814 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 823 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 185 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 190 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 187 |
super(measurements, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1797 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3379 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1173 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1987 |
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1879 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3417 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1137 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1959 |
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyMagneticFluxDensity> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 902 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 900 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 905 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 704 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 909 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 814 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 185 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 823 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 190 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 187 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 902 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 900 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 905 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1790 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1791 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets ground truth gravity norm to be expected at location where measurements have been made,
* expressed in meter per squared second (m/s^2).
*
* @return ground truth gravity norm or null.
*/
public Double getGroundTruthGravityNorm() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1744 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1835 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @return position where body kinematics measures have been taken.
*/
public ECEFPosition getEcefPosition() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2100 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2059 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2189 |
&& groundTruthGravityNorm != null;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3092 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4110 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated position.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Internal method to perform calibration when common z-axis is assumed for both
* the accelerometer and gyroscope.
*
* @throws AlgebraException if there are numerical errors.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateCommonAxis() throws AlgebraException, FittingException, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4409 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2477 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1835 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1791 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4743 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2699 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1790 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1744 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1167 |
initialBiasZ = initialBias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1797 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3379 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1137 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1959 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 1952 |
hardIronZ = hardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1879 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3417 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1173 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1924 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1987 |
initialHardIronZ = initialHardIron.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMm() {
Matrix result;
try {
result = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
getInitialMm(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMm(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMm initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMm(final Matrix initialMm) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMm.getRows() != BodyKinematics.COMPONENTS || initialMm.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMm.getElementAtIndex(0);
initialMyx = initialMm.getElementAtIndex(1);
initialMzx = initialMm.getElementAtIndex(2);
initialMxy = initialMm.getElementAtIndex(3);
initialSy = initialMm.getElementAtIndex(4);
initialMzy = initialMm.getElementAtIndex(5);
initialMxz = initialMm.getElementAtIndex(6);
initialMyz = initialMm.getElementAtIndex(7);
initialSz = initialMm.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4409 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4743 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2477 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2699 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1671 |
biasZ = bias.getElementAtIndex(2);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
}
/**
* Gets a collection of body kinematics measurements taken at
* a given position with different unknown orientations and containing
* the standard deviations of accelerometer and gyroscope measurements.
*
* @return collection of body kinematics measurements at a known position
* with unknown orientations.
*/
@Override
public Collection<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2055 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2013 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2160 |
&& groundTruthGravityNorm != null;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2794 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4055 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2190 |
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each sequence.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2055 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2059 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2189 |
&& groundTruthGravityNorm != null;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2013 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2100 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2160 |
return measurements != null && measurements.size() >= getMinimumRequiredMeasurements() && position != null;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2056 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2014 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2767 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4003 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2161 |
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1671 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1790 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1744 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1835 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1791 |
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial scale factors and cross coupling errors matrix.
*/
@Override
public Matrix getInitialMa() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
getInitialMa(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param result instance where data will be stored.
* @throws IllegalArgumentException if provided matrix is not 3x3.
*/
@Override
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException {
if (running) {
throw new LockedException();
}
if (initialMa.getRows() != BodyKinematics.COMPONENTS || initialMa.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
initialSx = initialMa.getElementAtIndex(0);
initialMyx = initialMa.getElementAtIndex(1);
initialMzx = initialMa.getElementAtIndex(2);
initialMxy = initialMa.getElementAtIndex(3);
initialSy = initialMa.getElementAtIndex(4);
initialMzy = initialMa.getElementAtIndex(5);
initialMxz = initialMa.getElementAtIndex(6);
initialMyz = initialMa.getElementAtIndex(7);
initialSz = initialMa.getElementAtIndex(8);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2056 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2014 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2794 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4055 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2161 |
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2101 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2060 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2767 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4003 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2190 |
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1453 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1514 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2200 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2333 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2111 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2070 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2804 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4065 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1479 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2059 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3681 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2918 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3166 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1957 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2777 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4013 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2171 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2305 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2066 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2024 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2020 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1443 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1957 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2111 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2070 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2804 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2020 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4065 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1443 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2200 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2333 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2066 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2024 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1453 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2777 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4013 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1514 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1479 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2171 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2305 |
}
/**
* Returns amount of progress variation before notifying a progress change during
* calibration.
*
* @return amount of progress variation before notifying a progress change during
* calibration.
*/
public float getProgressDelta() {
return progressDelta;
}
/**
* Sets amount of progress variation before notifying a progress change during
* calibration.
*
* @param progressDelta amount of progress variation before notifying a progress
* change during calibration.
* @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setProgressDelta(final float progressDelta) throws LockedException {
if (running) {
throw new LockedException();
}
if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
throw new IllegalArgumentException();
}
this.progressDelta = progressDelta;
}
/**
* Returns amount of confidence expressed as a value between 0.0 and 1.0
* (which is equivalent to 100%). The amount of confidence indicates the probability
* that the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @return amount of confidence as a value between 0.0 and 1.0.
*/
public double getConfidence() {
return confidence;
}
/**
* Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
* equivalent to 100%). The amount of confidence indicates the probability that
* the estimated result is correct. Usually this value will be close to 1.0, but
* not exactly 1.0.
*
* @param confidence confidence to be set as a value between 0.0 and 1.0.
* @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
* @throws LockedException if calibrator is currently running.
*/
public void setConfidence(final double confidence) throws LockedException {
if (running) {
throw new LockedException();
}
if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
throw new IllegalArgumentException();
}
this.confidence = confidence;
}
/**
* Returns maximum allowed number of iterations. If maximum allowed number of
* iterations is achieved without converging to a result when calling calibrate(),
* a RobustEstimatorException will be raised.
*
* @return maximum allowed number of iterations.
*/
public int getMaxIterations() {
return maxIterations;
}
/**
* Sets maximum allowed number of iterations. When the maximum number of iterations
* is exceeded, result will not be available, however an approximate result will be
* available for retrieval.
*
* @param maxIterations maximum allowed number of iterations to be set.
* @throws IllegalArgumentException if provided value is less than 1.
* @throws LockedException if calibrator is currently running.
*/
public void setMaxIterations(final int maxIterations) throws LockedException {
if (running) {
throw new LockedException();
}
if (maxIterations < MIN_ITERATIONS) {
throw new IllegalArgumentException();
}
this.maxIterations = maxIterations;
}
/**
* Gets data related to inliers found after estimation.
*
* @return data related to inliers found after estimation.
*/
public InliersData getInliersData() {
return inliersData;
}
/**
* Indicates whether result must be refined using a non-linear solver over found inliers.
*
* @return true to refine result, false to simply use result found by robust estimator
* without further refining.
*/
public boolean isResultRefined() {
return refineResult;
}
/**
* Specifies whether result must be refined using a non-linear solver over found inliers.
*
* @param refineResult true to refine result, false to simply use result found by robust
* estimator without further refining.
* @throws LockedException if calibrator is currently running.
*/
public void setResultRefined(final boolean refineResult) throws LockedException {
if (running) {
throw new LockedException();
}
this.refineResult = refineResult;
}
/**
* Indicates whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @return true if covariance must be kept after refining result, false otherwise.
*/
public boolean isCovarianceKept() {
return keepCovariance;
}
/**
* Specifies whether covariance must be kept after refining result.
* This setting is only taken into account if result is refined.
*
* @param keepCovariance true if covariance must be kept after refining result,
* false otherwise.
* @throws LockedException if calibrator is currently running.
*/
public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
if (running) {
throw new LockedException();
}
this.keepCovariance = keepCovariance;
}
/**
* Returns quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation always returns null.
* Subclasses using quality scores must implement proper behavior.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return null;
}
/**
* Sets quality scores corresponding to each measurement.
* The larger the score value the better the quality of the sample.
* This implementation makes no action.
* Subclasses using quality scores must implement proper behaviour.
*
* @param qualityScores quality scores corresponding to each pair of
* matched points.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3075 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3095 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2837 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4113 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3436 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4367 |
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3704 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5039 |
return evaluateCommonAxis(i, params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var m11 = result[0];
final var m12 = result[1];
final var m22 = result[2];
final var m13 = result[3];
final var m23 = result[4];
final var m33 = result[5];
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mm.setElementAtIndex(0, m11);
mm.setElementAtIndex(1, 0.0);
mm.setElementAtIndex(2, 0.0);
mm.setElementAtIndex(3, m12);
mm.setElementAtIndex(4, m22);
mm.setElementAtIndex(5, 0.0);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33);
setResult(mm);
// at this point covariance is expressed in terms of M, and must
// be expressed in terms of Mg.
// We know that:
// Mg = M - I
// Gg = M * G
// M = [m11 m12 m13]
// [0 m22 m23]
// [0 0 m33]
// G = [g11 g12 g13] = 0
// [g21 g22 g23]
// [g31 g32 g33]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [0 m22 - 1 m23 ]
// [mzx mzy sz ] [0 0 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = [m11 m12 m13][g11 g12 g13]
// [gg21 gg22 gg23] [0 m22 m23][g21 g22 g23]
// [gg31 gg32 gg33] [0 0 m33][g31 g32 g33]
// Defining the linear application:
// F(M) = F(m11, m12, m22, m32=0, m13, m23, m33)
// as:
// [sx] = [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [0]
// [myz] [m23]
// [mzx] [0]
// [mzy] [0]
// [gg11] [0]
// [gg21] [0]
// [gg31] [0]
// [gg12] [0]
// [gg22] [0]
// [gg32] [0]
// [gg13] [0]
// [gg23] [0]
// [gg33] [0]
// Then the Jacobian of F(M) is:
// J = [1 0 0 0 0 0]
// [0 0 1 0 0 0]
// [0 0 0 0 0 1]
// [0 1 0 0 0 0]
// [0 0 0 1 0 0]
// [0 0 0 0 0 0]
// [0 0 0 0 1 0]
// [0 0 0 0 0 0]
// [0 0 0 0 0 0]
// [0 0 0 0 0 0]
// [0 0 0 0 0 0]
// [0 0 0 0 0 0]
// [0 0 0 0 0 0]
// [0 0 0 0 0 0]
// [0 0 0 0 0 0]
// [0 0 0 0 0 0]
// [0 0 0 0 0 0]
// [0 0 0 0 0 0]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS);
jacobian.setElementAt(0, 0, 1.0);
jacobian.setElementAt(1, 2, 1.0);
jacobian.setElementAt(2, 5, 1.0);
jacobian.setElementAt(3, 1, 1.0);
jacobian.setElementAt(4, 3, 1.0);
jacobian.setElementAt(6, 4, 1.0);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Internal method to perform general calibration when G-dependant cross
* biases are ignored.
*
* @throws AlgebraException if accelerometer parameters prevent fixing
* measured accelerometer values due to numerical instabilities.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateGeneral() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 6260 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5984 |
return create(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener,
DEFAULT_ROBUST_METHOD);
}
/**
* Computes error of preliminary result respect a given measurement for current gravity norm.
*
* @param measurement a measurement.
* @param preliminaryResult a preliminary result.
* @return computed error.
*/
protected double computeError(
final StandardDeviationBodyKinematics measurement, final PreliminaryResult preliminaryResult) {
try {
// We know that measured specific force is:
// fmeas = ba + (I + Ma) * ftrue
// fmeas - ba = (I + Ma) * ftrue
// ftrue = (I + Ma)^-1 * (fmeas - ba)
// We know that ||ftrue|| should be equal to the gravity value at current Earth
// position
// ||ftrue|| = g ~ 9.81 m/s^2
final var estBiases = preliminaryResult.estimatedBiases;
final var estMa = preliminaryResult.estimatedMa;
if (identity == null) {
identity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp1 == null) {
tmp1 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp2 == null) {
tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp3 == null) {
tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (tmp4 == null) {
tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
identity.add(estMa, tmp1);
Utils.inverse(tmp1, tmp2);
final var kinematics = measurement.getKinematics();
final var fmeasX = kinematics.getFx();
final var fmeasY = kinematics.getFy();
final var fmeasZ = kinematics.getFz();
final var bx = estBiases[0];
final var by = estBiases[1];
final var bz = estBiases[2];
tmp3.setElementAtIndex(0, fmeasX - bx);
tmp3.setElementAtIndex(1, fmeasY - by);
tmp3.setElementAtIndex(2, fmeasZ - bz);
tmp2.multiply(tmp3, tmp4);
final var norm = Utils.normF(tmp4);
final var diff = groundTruthGravityNorm - norm; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3077 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3438 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2955 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4192 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4369 |
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated x coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3097 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2839 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4115 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2955 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4192 |
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated position.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4110 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4677 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5460 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6059 |
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, GENERAL_UNKNOWNS_AND_CROSS_BIASES);
jacobian.setElementAt(0, 0, m11);
jacobian.setElementAt(0, 1, m12);
jacobian.setElementAt(0, 2, m13);
jacobian.setElementAt(0, 3, bx);
jacobian.setElementAt(0, 6, by);
jacobian.setElementAt(0, 9, bz);
jacobian.setElementAt(1, 0, m21);
jacobian.setElementAt(1, 1, m22);
jacobian.setElementAt(1, 2, m23);
jacobian.setElementAt(1, 4, bx);
jacobian.setElementAt(1, 7, by);
jacobian.setElementAt(1, 10, bz);
jacobian.setElementAt(2, 0, m31);
jacobian.setElementAt(2, 1, m32);
jacobian.setElementAt(2, 2, m33);
jacobian.setElementAt(2, 5, bx);
jacobian.setElementAt(2, 8, by);
jacobian.setElementAt(2, 11, bz);
jacobian.setElementAt(3, 3, 1.0);
jacobian.setElementAt(4, 7, 1.0);
jacobian.setElementAt(5, 11, 1.0);
jacobian.setElementAt(6, 6, 1.0);
jacobian.setElementAt(7, 9, 1.0);
jacobian.setElementAt(8, 4, 1.0);
jacobian.setElementAt(9, 10, 1.0);
jacobian.setElementAt(10, 5, 1.0);
jacobian.setElementAt(11, 8, 1.9); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3189 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2199 |
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets variance of estimated x coordinate of gyroscope bias expressed in (rad^2/s^2).
*
* @return variance of estimated x coordinate of gyroscope bias or null if not available.
*/
public Double getEstimatedBiasXVariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 5222 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2606 |
estimatedMa.setElementAt(i, i, estimatedMa.getElementAt(i, i) - 1.0);
}
// since only a constant term is subtracted, covariance is preserved
estimatedCovariance = fitter.getCovar();
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters for the general case.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param params array containing current parameters for the general purpose case.
* Must have length 9.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateGeneral(final double[] params) throws EvaluationException {
final var m11 = params[0];
final var m21 = params[1];
final var m31 = params[2];
final var m12 = params[3];
final var m22 = params[4];
final var m32 = params[5];
final var m13 = params[6];
final var m23 = params[7];
final var m33 = params[8];
return evaluate(m11, m21, m31, m12, m22, m32, m13, m23, m33);
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters when common z-axis is assumed.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param params array containing current parameters for the common z-axis case.
* Must have length 6.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateCommonAxis(final double[] params) throws EvaluationException {
final var m11 = params[0];
final var m12 = params[1];
final var m22 = params[2];
final var m13 = params[3];
final var m23 = params[4];
final var m33 = params[5];
return evaluate(m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33);
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param m11 element 1,1 of cross-coupling error matrix.
* @param m21 element 2,1 of cross-coupling error matrix.
* @param m31 element 3,1 of cross-coupling error matrix.
* @param m12 element 1,2 of cross-coupling error matrix.
* @param m22 element 2,2 of cross-coupling error matrix.
* @param m32 element 3,2 of cross-coupling error matrix.
* @param m13 element 1,3 of cross-coupling error matrix.
* @param m23 element 2,3 of cross-coupling error matrix.
* @param m33 element 3,3 of cross-coupling error matrix.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluate(final double m11, final double m21, final double m31,
final double m12, final double m22, final double m32,
final double m13, final double m23, final double m33) throws EvaluationException {
// fmeas = M*(ftrue + b)
// ftrue = M^-1*fmeas - b
try {
if (fmeas == null) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 335 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 339 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 338 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 341 |
final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5869 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6649 |
private double evaluate(final double m11, final double m21, final double m31,
final double m12, final double m22, final double m32,
final double m13, final double m23, final double m33) throws EvaluationException {
// Ωmeas = M*(Ωtrue + b)
// Ωtrue = M^-1 * Ωmeas - b
try {
if (measAngularRate == null) {
measAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (m == null) {
m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (invM == null) {
invM = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (b == null) {
b = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (trueAngularRate == null) {
trueAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
}
measAngularRate.setElementAtIndex(0, measAngularRateX);
measAngularRate.setElementAtIndex(1, measAngularRateY);
measAngularRate.setElementAtIndex(2, measAngularRateZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, biasX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1507 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1511 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 409 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 421 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the calibration.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5211 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2758 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3188 |
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1507 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 336 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 339 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1511 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 342 |
super(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2062 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2356 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3932 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3684 |
running = false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5213 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3190 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2136 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2245 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2203 |
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets variance of estimated x coordinate of accelerometer bias expressed in (m^2/s^4).
*
* @return variance of estimated x coordinate of accelerometer bias or null if not available.
*/
public Double getEstimatedBiasFxVariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1869 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1622 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2350 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2484 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2591 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2724 |
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bx, by, bz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets variance of estimated x coordinate of magnetometer bias expressed in
* squared Teslas (T^2).
*
* @return variance of estimated x coordinate of magnetometer bias or null if
* not available.
*/
public Double getEstimatedHardIronXVariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4672 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2760 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2136 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2245 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2203 |
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
}
/**
* Gets estimated chi square value.
*
* @return estimated chi square value.
*/
@Override
public double getEstimatedChiSq() {
return estimatedChiSq;
}
/**
* Gets estimated mean square error respect to provided measurements.
*
* @return estimated mean square error respect to provided measurements.
*/
@Override
public double getEstimatedMse() {
return estimatedMse;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1507 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 342 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1941 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 7918 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 6882 |
initialMm, listener, DEFAULT_ROBUST_METHOD);
}
/**
* Computes error of a preliminary result respect a given measurement.
*
* @param measurement a measurement.
* @param preliminaryResult a preliminary result.
* @return computed error.
*/
protected double computeError(
final StandardDeviationBodyMagneticFluxDensity measurement, final PreliminaryResult preliminaryResult) {
try {
// The magnetometer model is:
// mBmeas = bm + (I + Mm) * mBtrue
// mBmeas - bm = (I + Mm) * mBtrue
// mBtrue = (I + Mm)^-1 * (mBmeas - ba)
// We know that ||mBtrue||should be equal to the magnitude of the
// Earth magnetic field at provided location
final var estimatedBiases = preliminaryResult.estimatedHardIron;
final var estMm = preliminaryResult.estimatedMm;
if (identity == null) {
identity = Matrix.identity(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
}
if (tmp1 == null) {
tmp1 = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
}
if (tmp2 == null) {
tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp3 == null) {
tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (tmp4 == null) {
tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
identity.add(estMm, tmp1);
Utils.inverse(tmp1, tmp2);
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
final var bMeasX = measuredMagneticFluxDensity.getBx();
final var bMeasY = measuredMagneticFluxDensity.getBy();
final var bMeasZ = measuredMagneticFluxDensity.getBz();
final var bx = estimatedBiases[0];
final var by = estimatedBiases[1];
final var bz = estimatedBiases[2];
tmp3.setElementAtIndex(0, bMeasX - bx);
tmp3.setElementAtIndex(1, bMeasY - by);
tmp3.setElementAtIndex(2, bMeasZ - bz);
tmp2.multiply(tmp3, tmp4);
final var norm = Utils.normF(tmp4);
final var diff = groundTruthMagneticFluxDensityNorm - norm; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1948 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1940 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1944 |
super(position, measurements, commonAxisUsed, hardIron, initialMm, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates magnetometer calibration parameters containing soft-iron
* scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java | 1856 |
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java | 1587 |
state = new INSLooselyCoupledKalmanState();
}
}
/**
* Corrects provided kinematics by taking into account currently estimated
* specific force and angular rate biases.
* This method stores the result into the variable member containing corrected
* kinematics values.
*
* @param kinematics kinematics instance to be corrected.
*/
private void correctKinematics(final BodyKinematics kinematics) {
if (correctedKinematics == null) {
correctedKinematics = new BodyKinematics();
}
final double accelBiasX;
final double accelBiasY;
final double accelBiasZ;
final double gyroBiasX;
final double gyroBiasY;
final double gyroBiasZ;
if (state != null) {
accelBiasX = getValueOrZero(state.getAccelerationBiasX());
accelBiasY = getValueOrZero(state.getAccelerationBiasY());
accelBiasZ = getValueOrZero(state.getAccelerationBiasZ());
gyroBiasX = getValueOrZero(state.getGyroBiasX());
gyroBiasY = getValueOrZero(state.getGyroBiasY());
gyroBiasZ = getValueOrZero(state.getGyroBiasZ());
} else {
accelBiasX = 0.0;
accelBiasY = 0.0;
accelBiasZ = 0.0;
gyroBiasX = 0.0;
gyroBiasY = 0.0;
gyroBiasZ = 0.0;
}
final var fx = kinematics.getFx();
final var fy = kinematics.getFy();
final var fz = kinematics.getFz();
final var angularRateX = kinematics.getAngularRateX();
final var angularRateY = kinematics.getAngularRateY();
final var angularRateZ = kinematics.getAngularRateZ();
correctedKinematics.setSpecificForceCoordinates(fx - accelBiasX, fy - accelBiasY, fz - accelBiasZ);
correctedKinematics.setAngularRateCoordinates(
angularRateX - gyroBiasX,
angularRateY - gyroBiasY,
angularRateZ - gyroBiasZ);
}
/**
* Returns provided value if not infinity and not NaN.
*
* @param value value to be returned.
* @return value or 0.0.
*/
private double getValueOrZero(final double value) {
if (Double.isNaN(value) || Double.isInfinite(value)) {
return 0.0;
} else {
return value;
}
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1058 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1194 |
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
// estimate Earth magnetic flux density at frame position and
// timestamp using WMM
final var ecefFrame = measurement.getFrame();
ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);
final var year = measurement.getYear();
final var latitude = nedFrame.getLatitude();
final var longitude = nedFrame.getLongitude();
final var height = nedFrame.getHeight();
nedFrame.getCoordinateTransformation(cbn);
cbn.inverse(cnb);
wmmEstimator.estimate(latitude, longitude, height, year, earthB);
// estimate expected body magnetic flux density taking into
// account body attitude (inverse of frame orientation) and
// estimated Earth magnetic flux density
BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);
final var bMeasX = measuredMagneticFluxDensity.getBx();
final var bMeasY = measuredMagneticFluxDensity.getBy();
final var bMeasZ = measuredMagneticFluxDensity.getBz();
final var bTrueX = expectedMagneticFluxDensity.getBx();
final var bTrueY = expectedMagneticFluxDensity.getBy();
final var bTrueZ = expectedMagneticFluxDensity.getBz();
a.setElementAt(i, 0, 1.0);
a.setElementAt(i, 3, bTrueX);
a.setElementAt(i, 6, bTrueY);
a.setElementAt(i, 7, bTrueZ);
b.setElementAtIndex(i, bMeasX - bTrueX);
i++;
a.setElementAt(i, 1, 1.0);
a.setElementAt(i, 4, bTrueY);
a.setElementAt(i, 8, bTrueZ); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3396 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3459 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1948 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1152 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3092 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2834 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4110 |
} catch (final AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 973 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 977 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2917 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2367 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
}
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Gets known accelerometer bias as a column matrix.
*
* @return known accelerometer bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known accelerometer bias as a column matrix.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5958 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 5480 |
estimatedMg = preliminaryResult.estimatedMg;
estimatedGg = preliminaryResult.estimatedGg;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
}
/**
* Converts acceleration instance to meters per squared second.
*
* @param acceleration acceleration instance to be converted.
* @return converted value.
*/
private static double convertAcceleration(final Acceleration acceleration) {
return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Converts angular speed value and unit to radians per second.
*
* @param value angular speed value.
* @param unit unit of angular speed value.
* @return converted value.
*/
private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Converts angular speed instance to radians per second.
*
* @param angularSpeed angular speed instance to be converted.
* @return converted value.
*/
private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
}
/**
* Fixes a measured kinematics instance using current
*
* @param measuredKinematics a measured kinematics instance.
* @param result instance where fixed values will be stored.
* @throws AlgebraException if accelerometer or gyroscope parameters
* contain numerical instabilities.
*/
private void fixKinematics(final BodyKinematics measuredKinematics, final BodyKinematics result)
throws AlgebraException {
measuredSpecificForce[0] = measuredKinematics.getFx();
measuredSpecificForce[1] = measuredKinematics.getFy();
measuredSpecificForce[2] = measuredKinematics.getFz();
accelerationFixer.fix(measuredSpecificForce, fixedSpecificForce);
measuredAngularRate[0] = measuredKinematics.getAngularRateX();
measuredAngularRate[1] = measuredKinematics.getAngularRateY();
measuredAngularRate[2] = measuredKinematics.getAngularRateZ();
angularRateFixer.fix(measuredAngularRate, fixedSpecificForce, fixedAngularRate);
result.setSpecificForceCoordinates(fixedSpecificForce[0], fixedSpecificForce[1], fixedSpecificForce[2]);
result.setAngularRateCoordinates(fixedAngularRate[0], fixedAngularRate[1], fixedAngularRate[2]);
}
/**
* Internal class containing estimated preliminary result.
*/
protected static class PreliminaryResult {
/**
* Estimated gyroscope biases for each IMU axis expressed in radians per second
* (rad/s).
*/
private double[] estimatedBiases; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3095 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2837 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4113 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3187 |
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated position.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3077 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3097 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2839 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4115 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3438 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2199 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4369 |
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3189 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2955 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4192 |
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): bgx, bgy, bgz, sx, sy, sz,
* mxy, mxz, myx, myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32,
* gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated parameters.
*/
@Override
public Matrix getEstimatedCovariance() {
return estimatedCovariance;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1507 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1571 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 336 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1593 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1511 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 339 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 342 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1941 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2770 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2739 |
public void setListener(final EasyGyroscopeCalibratorListener listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required sequences.
*
* @return minimum number of required sequences.
*/
@Override
public int getMinimumRequiredMeasurementsOrSequences() {
if (commonAxisUsed) {
if (estimateGDependentCrossBiases) {
return MINIMUM_SEQUENCES_COMMON_Z_AXIS_AND_CROSS_BIASES;
} else {
return MINIMUM_SEQUENCES_COMMON_Z_AXIS;
}
} else {
if (estimateGDependentCrossBiases) {
return MINIMUM_SEQUENCES_GENERAL_AND_CROSS_BIASES;
} else {
return MINIMUM_SEQUENCES_GENERAL;
}
}
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return sequences != null && sequences.size() >= getMinimumRequiredMeasurementsOrSequences();
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
reset();
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
if (estimateGDependentCrossBiases) {
calibrateCommonAxisAndGDependentCrossBiases();
} else {
calibrateCommonAxis();
}
} else {
if (estimateGDependentCrossBiases) {
calibrateGeneralAndGDependentCrossBiases();
} else {
calibrateGeneral();
}
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final FittingException | AlgebraException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated gyroscope biases
* expressed in radians per second (rad/s).
*
* @return array containing x,y,z components of estimated gyroscope biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4016 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4063 |
final KnownBiasTurntableGyroscopeCalibratorListener listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurementsOrSequences() {
if (commonAxisUsed) {
if (estimateGDependentCrossBiases) {
return MINIMUM_MEASUREMENTS_COMMON_Z_AXIS_AND_CROSS_BIASES;
} else {
return MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;
}
} else {
if (estimateGDependentCrossBiases) {
return MINIMUM_MEASUREMENTS_GENERAL_AND_CROSS_BIASES;
} else {
return MINIMUM_MEASUREMENTS_GENERAL;
}
}
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= getMinimumRequiredMeasurementsOrSequences();
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
if (estimateGDependentCrossBiases) {
calibrateCommonAxisAndGDependentCrossBiases();
} else {
calibrateCommonAxis();
}
} else {
if (estimateGDependentCrossBiases) {
calibrateGeneralAndGDependentCrossBiases();
} else {
calibrateGeneral();
}
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException
| InvalidSourceAndDestinationFrameTypeException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1928 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3681 |
} catch (final AlgebraException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4667 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1148 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2755 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3095 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2837 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4113 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1899 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4448 |
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated position.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2059 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1928 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 5723 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 5473 |
return create(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener,
DEFAULT_ROBUST_METHOD);
}
/**
* Computes error of preliminary result respect a given measurement for current gravity norm.
*
* @param measurement a measurement.
* @param preliminaryResult a preliminary result.
* @return computed error.
*/
protected double computeError(
final StandardDeviationBodyKinematics measurement, final PreliminaryResult preliminaryResult) {
try {
// We know that measured specific force is:
// fmeas = ba + (I + Ma) * ftrue
// fmeas - ba = (I + Ma) * ftrue
// ftrue = (I + Ma)^-1 * (fmeas - ba)
// We know that ||ftrue|| should be equal to the gravity value at current Earth
// position
// ||ftrue|| = g ~ 9.81 m/s^2
final var estMa = preliminaryResult.estimatedMa;
if (identity == null) {
identity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp1 == null) {
tmp1 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp2 == null) {
tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp3 == null) {
tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (tmp4 == null) {
tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
identity.add(estMa, tmp1);
Utils.inverse(tmp1, tmp2);
final var kinematics = measurement.getKinematics();
final var fmeasX = kinematics.getFx();
final var fmeasY = kinematics.getFy();
final var fmeasZ = kinematics.getFz();
tmp3.setElementAtIndex(0, fmeasX - biasX);
tmp3.setElementAtIndex(1, fmeasY - biasY);
tmp3.setElementAtIndex(2, fmeasZ - biasZ);
tmp2.multiply(tmp3, tmp4);
final var norm = Utils.normF(tmp4);
final var diff = groundTruthGravityNorm - norm; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2199 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2955 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4192 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1901 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4450 |
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
}
/**
* Gets estimated covariance matrix for estimated calibration solution.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy, gg11, gg21, gg31, gg12, gg22, gg32, gg13, gg23, gg33.
* This is only available when result has been refined and covariance
* is kept.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3075 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1155 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3436 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3187 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1899 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4448 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4367 |
return false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated gyroscope x-axis scale factor.
*
* @return estimated gyroscope x-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated gyroscope y-axis scale factor.
*
* @return estimated gyroscope y-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated gyroscope z-axis scale factor.
*
* @return estimated gyroscope z-axis scale factor or null
* if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated gyroscope x-y cross-coupling error.
*
* @return estimated gyroscope x-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated gyroscope x-z cross-coupling error.
*
* @return estimated gyroscope x-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated gyroscope y-x cross-coupling error.
*
* @return estimated gyroscope y-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated gyroscope y-z cross-coupling error.
*
* @return estimated gyroscope y-z cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated gyroscope z-x cross-coupling error.
*
* @return estimated gyroscope z-x cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated gyroscope z-y cross-coupling error.
*
* @return estimated gyroscope z-y cross-coupling error or null
* if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
* This instance allows any 3x3 matrix.
*
* @return estimated G-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1436 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 982 |
final var g33 = unknowns.getElementAtIndex(14);
fillMg(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
fillGg(g11, g12, g13, g21, g22, g23, g31, g32, g33);
}
/**
* Internal method to perform general calibration.
*
* @throws AlgebraException if there are numerical errors.
*/
private void calibrateGeneral() throws AlgebraException {
// The gyroscope model is:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
// Hence:
// [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1 0] [myx sy myz] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1] [mzx mzy sz ] [Ωtruez] [g31 g32 g33][ftruez]
// [Ωmeasx] = [bx] + ( [1+sx mxy mxz ]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [myx 1+sy myz ] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [mzx mzy 1+sz] [Ωtruez] [g31 g32 g33][ftruez]
// Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + myx * Ωtruex + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy, g11, g12, g13, g21, g22, g23,
// g31, g32, g33
// Reordering:
// Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy - Ωtruey - by = myx * Ωtruex + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz - Ωtruez - bz = mzx * Ωtruex + mzy * Ωtruey + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// [Ωtruex 0 0 Ωtruey Ωtruez 0 0 0 0 ftruex ftruey ftruez 0 0 0 0 0 0 ][sx ] = [Ωmeasx - Ωtruex - bx]
// [0 Ωtruey 0 0 0 Ωtruex Ωtruez 0 0 0 0 0 ftruex ftruey ftruez 0 0 0 ][sy ] [Ωmeasy - Ωtruey - by]
// [0 0 Ωtruez 0 0 0 0 Ωtruex Ωtruey 0 0 0 0 0 0 ftruex ftruey ftruez][sz ] [Ωmeasz - Ωtruez - bz]
// [mxy]
// [mxz]
// [myx]
// [myz]
// [mzx]
// [mzy]
// [g11]
// [g12]
// [g13]
// [g21]
// [g22]
// [g23]
// [g31]
// [g32]
// [g33]
final var expectedKinematics = new BodyKinematics();
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var omegaMeasX = measuredKinematics.getAngularRateX();
final var omegaMeasY = measuredKinematics.getAngularRateY();
final var omegaMeasZ = measuredKinematics.getAngularRateZ();
final var omegaTrueX = expectedKinematics.getAngularRateX();
final var omegaTrueY = expectedKinematics.getAngularRateY();
final var omegaTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, omegaTrueX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3095 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2837 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4113 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 690 |
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1157 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 692 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2199 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2955 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4192 |
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() {
return estimatedMg;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMg != null ? estimatedMg.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMg != null ? estimatedMg.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMg != null ? estimatedMg.getElementAt(2, 1) : null;
}
/**
* Gets estimated G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*
* @return a 3x3 matrix containing g-dependent cross biases.
*/
@Override
public Matrix getEstimatedGg() {
return estimatedGg;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5748 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6518 |
private double evaluate(final double m11, final double m21, final double m31,
final double m12, final double m22, final double m32,
final double m13, final double m23, final double m33,
final double g11, final double g21, final double g31,
final double g12, final double g22, final double g32,
final double g13, final double g23, final double g33) throws EvaluationException {
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
// Ωmeas = M*(Ωtrue + b + G * ftrue)
// M = I + Mg
// bg = M*b --> b = M^-1*bg
// Gg = M*G --> G = M^-1*Gg
// Ωtrue = M^-1 * Ωmeas - b - G*ftrue
try {
if (measAngularRate == null) {
measAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (fmeas == null) {
fmeas = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (m == null) {
m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (invM == null) {
invM = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (b == null) {
b = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (g == null) {
g = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (trueAngularRate == null) {
trueAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (ftrue == null) {
ftrue = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (tmp == null) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2138 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2276 |
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
// estimate Earth magnetic flux density at frame position and
// timestamp using WMM
final var ecefFrame = measurement.getFrame();
ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);
final var year = measurement.getYear();
final var latitude = nedFrame.getLatitude();
final var longitude = nedFrame.getLongitude();
final var height = nedFrame.getHeight();
nedFrame.getCoordinateTransformation(cbn);
cbn.inverse(cnb);
wmmEstimator.estimate(latitude, longitude, height, year, earthB);
// estimate expected body magnetic flux density taking into
// account body attitude (inverse of frame orientation) and
// estimated Earth magnetic flux density
BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);
final var bMeasX = measuredMagneticFluxDensity.getBx();
final var bMeasY = measuredMagneticFluxDensity.getBy();
final var bMeasZ = measuredMagneticFluxDensity.getBz();
final var bTrueX = expectedMagneticFluxDensity.getBx();
final var bTrueY = expectedMagneticFluxDensity.getBy();
final var bTrueZ = expectedMagneticFluxDensity.getBz();
a.setElementAt(i, 0, bTrueX);
a.setElementAt(i, 1, 0.0);
a.setElementAt(i, 2, 0.0);
a.setElementAt(i, 3, bTrueY);
a.setElementAt(i, 4, bTrueZ);
a.setElementAt(i, 5, 0.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6123 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3260 |
setResult(crossCoupling, bias);
// at this point covariance is expressed in terms of b and M, and must
// be expressed in terms of ba and Ma.
// We know that:
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [0 m22 m23]
// [0 0 m33]
// m21 = m31 = m32 = 0
// and that ba and Ma are expressed as:
// Ma = M - I
// ba = M * b
// Ma = [m11 - 1 m12 m13 ] = [sx mxy mxz]
// [0 m22 - 1 m23 ] [0 sy myz]
// [0 0 m33 - 1] [0 0 sz ]
// ba = [m11 * bx + m12 * by + m13 * bz] = [bax]
// [ m22 * by + m23 * bz] [bay]
// [ m33 * bz] [baz]
// Defining the linear application:
// F(b, M) = F(bx, by, bz, m11, m12, m22, m13, m23, m33)
// as:
// [bax] = [m11 * bx + m12 * by + m13 * bz]
// [bay] [m22 * by + m23 * bz]
// [baz] [m33 * bz]
// [sx] [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 -1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [0]
// [myz] [m23]
// [mzx] [0]
// [mzy] [0]
// Then the Jacobian of F(b, M) is:
// J = [m11 m12 m13 bx by 0 bz 0 0 ]
// [0 m22 m23 0 0 by 0 bz 0 ]
// [0 0 m33 0 0 0 0 0 bz]
// [0 0 0 1 0 0 0 0 0 ]
// [0 0 0 0 0 1 0 0 0 ]
// [0 0 0 0 0 0 0 0 1 ]
// [0 0 0 0 1 0 0 0 0 ]
// [0 0 0 0 0 0 1 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 1 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
jacobian.setElementAt(0, 0, m11);
jacobian.setElementAt(0, 1, m12);
jacobian.setElementAt(1, 1, m22);
jacobian.setElementAt(0, 2, m13);
jacobian.setElementAt(1, 2, m23);
jacobian.setElementAt(2, 2, m33);
jacobian.setElementAt(0, 3, bx);
jacobian.setElementAt(3, 3, 1.0);
jacobian.setElementAt(0, 4, by);
jacobian.setElementAt(6, 4, 1.0);
jacobian.setElementAt(1, 5, by);
jacobian.setElementAt(4, 5, 1.0);
jacobian.setElementAt(0, 6, bz);
jacobian.setElementAt(7, 6, 1.0);
jacobian.setElementAt(1, 7, bz);
jacobian.setElementAt(9, 7, 1.0);
jacobian.setElementAt(2, 8, bz);
jacobian.setElementAt(5, 8, 1.0);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Makes proper conversion of internal cross-coupling and bias matrices.
*
* @param m internal cross-coupling matrix.
* @param b internal bias matrix.
* @throws AlgebraException if a numerical instability occurs.
*/
private void setResult(final Matrix m, final Matrix b) throws AlgebraException {
// Because:
// M = I + Ma
// b = M^-1*ba
// Then:
// Ma = M - I
// ba = M*b
if (estimatedBiases == null) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4354 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5717 |
setResult(m, b);
// at this point covariance is expressed in terms of b, M and G, and must
// be expressed in terms of bg, Mg and Gg.
// We know that:
// bg = M * b
// Mg = M - I
// Gg = M * G = 0
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [0 m22 m23]
// [0 0 m33]
// G = [g11 g12 g13] = 0
// [g21 g22 g23]
// [g31 g32 g33]
// bg = [m11 m12 m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
// [0 m22 m23][by] [ m22 * by + m23 * bz] [bgy]
// [0 0 m33][bz] [ m33 * bz] [bgz]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [0 m22 - 1 m23 ]
// [mzx mzy sz ] [0 0 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = 0
// [gg21 gg22 gg23]
// [gg31 gg32 gg33]
// Defining the linear application:
// F(b, M) = F(bx, by, bz, m11, m12, m22, m13, m23, m33)
// as:
// [bgx] = [m11 * bx + m12 * by + m13 * bz]
// [bgy] [ m22 * by + m23 * bz]
// [bgz] [ m33 * bz]
// [sx] [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [0]
// [myz] [m23]
// [mzx] [0]
// [mzy] [0]
// [gg11] [0]
// [gg21] [0]
// [gg31] [0]
// [gg12] [0]
// [gg22] [0]
// [gg32] [0]
// [gg13] [0]
// [gg23] [0]
// [gg33] [0]
// Then the Jacobian of F(b, M) is:
// J = [m11 m12 m13 bx by 0 bz 0 0 ]
// [0 m22 m23 0 0 by 0 bz 0 ]
// [0 0 m33 0 0 0 0 0 bz ]
// [0 0 0 1 0 0 0 0 0 ]
// [0 0 0 0 0 1 0 0 0 ]
// [0 0 0 0 0 0 0 0 1 ]
// [0 0 0 0 1 0 0 0 0 ]
// [0 0 0 0 0 0 1 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 1 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 ]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS);
jacobian.setElementAt(0, 0, m11);
jacobian.setElementAt(0, 1, m12);
jacobian.setElementAt(0, 2, m13);
jacobian.setElementAt(0, 3, bx);
jacobian.setElementAt(0, 4, by);
jacobian.setElementAt(0, 6, bz);
jacobian.setElementAt(1, 1, m22);
jacobian.setElementAt(1, 2, m23);
jacobian.setElementAt(1, 5, by);
jacobian.setElementAt(1, 7, bz);
jacobian.setElementAt(2, 2, m33);
jacobian.setElementAt(2, 8, bz);
jacobian.setElementAt(3, 3, 1.0);
jacobian.setElementAt(4, 5, 1.0);
jacobian.setElementAt(5, 8, 1.0);
jacobian.setElementAt(6, 4, 1.0);
jacobian.setElementAt(7, 6, 1.0);
jacobian.setElementAt(9, 7, 1.0);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Internal method to perform general calibration when G-dependant cross
* biases are ignored.
*
* @throws AlgebraException if accelerometer parameters prevent fixing
* measured accelerometer values due to numerical instabilities.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateGeneral() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2758 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1837 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2495 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2454 |
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2062 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3684 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1867 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2589 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2722 |
running = false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 5228 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5665 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2612 |
estimatedMse = fitter.getMse();
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters for the general case.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param params array containing current parameters for the general purpose case.
* Must have length 9.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateGeneral(final double[] params) throws EvaluationException {
final var m11 = params[0];
final var m21 = params[1];
final var m31 = params[2];
final var m12 = params[3];
final var m22 = params[4];
final var m32 = params[5];
final var m13 = params[6];
final var m23 = params[7];
final var m33 = params[8];
return evaluate(m11, m21, m31, m12, m22, m32, m13, m23, m33);
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters when common z-axis is assumed.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param params array containing current parameters for the common z-axis case.
* Must have length 6.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateCommonAxis(final double[] params) throws EvaluationException {
final var m11 = params[0];
final var m12 = params[1];
final var m22 = params[2];
final var m13 = params[3];
final var m23 = params[4];
final var m33 = params[5];
return evaluate(m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33);
}
/**
* Computes estimated true specific force squared norm using current measured
* specific force and provided parameters.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param m11 element 1,1 of cross-coupling error matrix.
* @param m21 element 2,1 of cross-coupling error matrix.
* @param m31 element 3,1 of cross-coupling error matrix.
* @param m12 element 1,2 of cross-coupling error matrix.
* @param m22 element 2,2 of cross-coupling error matrix.
* @param m32 element 3,2 of cross-coupling error matrix.
* @param m13 element 1,3 of cross-coupling error matrix.
* @param m23 element 2,3 of cross-coupling error matrix.
* @param m33 element 3,3 of cross-coupling error matrix.
* @return estimated true specific force squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluate(final double m11, final double m21, final double m31,
final double m12, final double m22, final double m32,
final double m13, final double m23, final double m33) throws EvaluationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2136 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2245 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2203 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1839 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2497 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2456 |
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
* This is only available when result has been refined and covariance
* is kept.
*
* @return estimated covariance matrix for estimated calibration parameters.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7467 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8596 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7946 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9056 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2064 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2358 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3934 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3686 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1622 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2350 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2484 |
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
}
/**
* Gets estimated covariance matrix for estimated calibration parameters.
* Diagonal elements of the matrix contains variance for the following
* parameters (following indicated order): sx, sy, sz, mxy, mxz, myx,
* myz, mzx, mzy.
*
* @return estimated covariance matrix for estimated position.
*/
@Override
public Matrix getEstimatedCovariance() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2758 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 679 |
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5211 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1151 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3188 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1837 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2495 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2454 |
return false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2062 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 848 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3684 |
running = false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2356 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3932 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1931 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1867 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2589 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2722 |
return false;
}
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1414 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1385 |
PROSACRobustEasyGyroscopeCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
setupAccelerationFixer();
inliersData = null;
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < EasyGyroscopeCalibrator.MINIMUM_SEQUENCES_COMMON_Z_AXIS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 6554 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 7060 |
}
}
/**
* Converts a time instance expressed in milliseconds since epoch time
* (January 1st, 1970 at midnight) to a decimal year.
*
* @param timestampMillis milliseconds value to be converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final Long timestampMillis) {
if (timestampMillis == null) {
return null;
}
final var calendar = new GregorianCalendar();
calendar.setTimeInMillis(timestampMillis);
return convertTime(calendar);
}
/**
* Converts a time instant contained ina date object to a
* decimal year.
*
* @param date a time instance to be converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final Date date) {
if (date == null) {
return null;
}
final var calendar = new GregorianCalendar();
calendar.setTime(date);
return convertTime(calendar);
}
/**
* Converts a time instant contained in a gregorian calendar to a
* decimal year.
*
* @param calendar calendar containing a specific instant to be
* converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final GregorianCalendar calendar) {
if (calendar == null) {
return null;
}
return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
}
/**
* Converts provided ECEF position to position expressed in NED
* coordinates.
*
* @param position ECEF position to be converted.
* @return converted position expressed in NED coordinates.
*/
private static NEDPosition convertPosition(final ECEFPosition position) {
final var velocity = new NEDVelocity();
final var result = new NEDPosition();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
position.getX(), position.getY(), position.getZ(),
0.0, 0.0, 0.0, result, velocity);
return result;
}
/**
* Converts magnetic flux density value and unit to Teslas.
*
* @param value magnetic flux density value.
* @param unit unit of magnetic flux density value.
* @return converted value.
*/
private static double convertMagneticFluxDensity(final double value, final MagneticFluxDensityUnit unit) {
return MagneticFluxDensityConverter.convert(value, unit, MagneticFluxDensityUnit.TESLA);
}
/**
* Converts magnetic flux density instance to Teslas.
*
* @param magneticFluxDensity magnetic flux density instance to be converted.
* @return converted value.
*/
private static double convertMagneticFluxDensity(final MagneticFluxDensity magneticFluxDensity) {
return convertMagneticFluxDensity(magneticFluxDensity.getValue().doubleValue(), magneticFluxDensity.getUnit());
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 936 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1229 |
this.biasZ = convertAcceleration(biasZ);
}
/**
* Gets known accelerometer bias.
*
* @return known accelerometer bias.
*/
@Override
public AccelerationTriad getBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known accelerometer bias.
*
* @param result instance where result will be stored.
*/
@Override
public void getBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known accelerometer bias.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBias(final AccelerationTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1153 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 681 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2136 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2245 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2203 |
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() {
return estimatedMa;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMa != null ? estimatedMa.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMa != null ? estimatedMa.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMa != null ? estimatedMa.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 850 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1933 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1622 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2350 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2484 |
}
/**
* Gets estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated magnetometer soft-iron scale factors and cross coupling errors,
* or null if not available.
*/
@Override
public Matrix getEstimatedMm() {
return estimatedMm;
}
/**
* Gets estimated x-axis scale factor.
*
* @return estimated x-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSx() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 0) : null;
}
/**
* Gets estimated y-axis scale factor.
*
* @return estimated y-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSy() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 1) : null;
}
/**
* Gets estimated z-axis scale factor.
*
* @return estimated z-axis scale factor or null if not available.
*/
@Override
public Double getEstimatedSz() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 2) : null;
}
/**
* Gets estimated x-y cross-coupling error.
*
* @return estimated x-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxy() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 1) : null;
}
/**
* Gets estimated x-z cross-coupling error.
*
* @return estimated x-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMxz() {
return estimatedMm != null ? estimatedMm.getElementAt(0, 2) : null;
}
/**
* Gets estimated y-x cross-coupling error.
*
* @return estimated y-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyx() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 0) : null;
}
/**
* Gets estimated y-z cross-coupling error.
*
* @return estimated y-z cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMyz() {
return estimatedMm != null ? estimatedMm.getElementAt(1, 2) : null;
}
/**
* Gets estimated z-x cross-coupling error.
*
* @return estimated z-x cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzx() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 0) : null;
}
/**
* Gets estimated z-y cross-coupling error.
*
* @return estimated z-y cross-coupling error or null if not available.
*/
@Override
public Double getEstimatedMzy() {
return estimatedMm != null ? estimatedMm.getElementAt(2, 1) : null;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 538 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 538 |
PROSACRobustKnownFrameMagnetometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
setupWmmEstimator();
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2143 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2142 |
PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
initialize();
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3621 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3585 |
public void setListener(final KnownFrameMagnetometerNonLinearLeastSquaresCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the estimator.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or no.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Gets Earth's magnetic model.
*
* @return Earth's magnetic model or null if not provided.
*/
public WorldMagneticModel getMagneticModel() {
return magneticModel;
}
/**
* Sets Earth's magnetic model.
*
* @param magneticModel Earth's magnetic model to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
if (running) {
throw new LockedException();
}
this.magneticModel = magneticModel;
}
/**
* Estimates magnetometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @return array containing x,y,z components of estimated magnetometer
* hard-iron biases.
*/
@Override
public double[] getEstimatedHardIron() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 163 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 169 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 166 |
final RobustKnownFrameAccelerometerCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1869 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1773 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1889 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1795 |
PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than the minimum required
* number of samples (10 or 13).
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < getMinimumRequiredMeasurements()) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java | 575 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java | 560 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return sequences.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(sequences.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustEasyGyroscopeCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 879 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 884 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 2007 |
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 2049 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1137 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1179 |
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final double x, final double y, final double z, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC,
SpeedConverter.convert(vx.getValue().doubleValue(), vx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(vy.getValue().doubleValue(), vy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(vz.getValue().doubleValue(), vz.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
x, y, z, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4879 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5694 |
}
/**
* Sets input data into Levenberg-Marquardt fitter.
*
* @throws WrongSizeException never happens.
*/
private void setInputData() throws WrongSizeException {
final var g = groundTruthGravityNorm;
final var g2 = g * g;
final var numMeasurements = measurements.size();
final var x = new Matrix(numMeasurements, BodyKinematics.COMPONENTS);
final var y = new double[numMeasurements];
final var specificForceStandardDeviations = new double[numMeasurements];
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var fx = measuredKinematics.getFx();
final var fy = measuredKinematics.getFy();
final var fz = measuredKinematics.getFz();
x.setElementAt(i, 0, fx);
x.setElementAt(i, 1, fy);
x.setElementAt(i, 2, fz);
y[i] = g2;
specificForceStandardDeviations[i] = measurement.getSpecificForceStandardDeviation();
i++;
}
fitter.setInputData(x, y, specificForceStandardDeviations);
}
/**
* Internal method to perform general calibration.
*
* @throws FittingException if Levenberg-Marquardt fails for numerical reasons.
* @throws AlgebraException if there are numerical instabilities that prevent
* matrix inversion.
* @throws com.irurueta.numerical.NotReadyException never happens.
*/
private void calibrateGeneral() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException {
// The accelerometer model is:
// fmeas = ba + (I + Ma) * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// fmeas = ba + (I + Ma) * ftrue
// For convergence purposes of the Levenberg-Marquardt algorithm, the
// accelerometer model can be better expressed as:
// fmeas = T*K*(ftrue + b)
// fmeas = M*(ftrue + b)
// fmeas = M*ftrue + M*b
// where:
// M = I + Ma
// ba = M*b = (I + Ma)*b --> b = M^-1*ba
// We know that the norm of the true specific force is equal to the amount
// of gravity at a certain Earth position
// ||ftrue|| = ||g|| ~ 9.81 m/s^2
// Hence:
// fmeas - M*b = M*ftrue
// M^-1 * (fmeas - M*b) = ftrue
// ||g||^2 = ||ftrue||^2 = (M^-1 * (fmeas - M*b))^T * (M^-1 * (fmeas - M*b))
// ||g||^2 = (fmeas - M*b)^T*(M^-1)^T * M^-1 * (fmeas - M*b)
// ||g||^2 = (fmeas - M * b)^T * ||M^-1||^2 * (fmeas - M * b)
// ||g||^2 = ||fmeas - M * b||^2 * ||M^-1||^2
// Where:
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [m21 m22 m23]
// [m31 m32 m33]
// Notice that bias b is known, hence only terms in matrix M need to be estimated
final var gradientEstimator = new GradientEstimator(this::evaluateGeneral);
final var initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
initialM.add(getInitialMa()); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 740 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java | 746 |
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
gravityNorm = computeGravityNorm();
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1704 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 535 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1710 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 539 |
PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 943 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2883 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1251 |
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Gets known gyroscope bias.
*
* @return known gyroscope bias.
*/
public AngularSpeedTriad getBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known gyroscope bias.
*
* @param result instance where result will be stored.
*/
public void getBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known gyroscope bias.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setBias(final AngularSpeedTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @return array containing coordinate of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known gyroscope bias as an array.
* Array values are expressed in radians per second (rad/s).
*
* @param bias known gyroscope bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2141 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2138 |
PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.this,
progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5292 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6329 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5813 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6830 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2266 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2837 |
private void setInputData() throws WrongSizeException {
final var gtb = groundTruthMagneticFluxDensityNorm;
final var gtb2 = gtb * gtb;
final var numMeasurements = measurements.size();
final var x = new Matrix(numMeasurements, BodyMagneticFluxDensity.COMPONENTS);
final var y = new double[numMeasurements];
final var specificForceStandardDeviations = new double[numMeasurements];
var i = 0;
for (final var measurement : measurements) {
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
final var bmeasuredX = measuredMagneticFluxDensity.getBx();
final var bmeasuredY = measuredMagneticFluxDensity.getBy();
final var bmeasuredZ = measuredMagneticFluxDensity.getBz();
x.setElementAt(i, 0, bmeasuredX);
x.setElementAt(i, 1, bmeasuredY);
x.setElementAt(i, 2, bmeasuredZ);
y[i] = gtb2;
specificForceStandardDeviations[i] = measurement.getMagneticFluxDensityStandardDeviation();
i++;
}
fitter.setInputData(x, y, specificForceStandardDeviations);
}
/**
* Internal method to perform general calibration.
*
* @throws FittingException if Levenberg-Marquardt fails for numerical reasons.
* @throws AlgebraException if there are numerical instabilities that prevent
* matrix inversion.
* @throws com.irurueta.numerical.NotReadyException never happens.
*/
private void calibrateGeneral() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException {
// The magnetometer model is:
// bmeas = bm + (I + Mm) * btrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// bmeas = bm + (I + Mm) * btrue
// For convergence purposes of the Levenberg-Marquardt algorithm, the
// magnetometer model can be better expressed as:
// bmeas = T*K*(btrue + b)
// bmeas = M*(btrue + b)
// bmeas = M*btrue + M*b
// where:
// M = I + Mm
// bm = M*b = (I + Mm)*b --> b = M^-1*bm
// We know that the norm of the true body magnetic flux density
// is equal to the amount of Earth magnetic flux density at provided
// position and timestamp
// ||btrue|| = ||bEarth|| --> from 30 µT to 60 µT
// Hence:
// bmeas - M*b = M*btrue
// M^-1 * (bmeas - M*b) = btrue
// ||bEarth||^2 = ||btrue||^2 = (M^-1 * (bmeas - M*b))^T * (M^-1 * (bmeas - M*b))
// ||bEarth||^2 = (bmeas - M*b)^T*(M^-1)^T * M^-1 * (bmeas - M*b)
// ||bEarth||^2 = (bmeas - M * b)^T * ||M^-1||^2 * (bmeas - M * b)
// ||bEarth||^2 = ||bmeas - M * b||^2 * ||M^-1||^2
// Where:
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [m21 m22 m23]
// [m31 m32 m33]
final var gradientEstimator = new GradientEstimator(this::evaluateGeneral);
final var initialM = Matrix.identity(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
initialM.add(getInitialMm()); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7631 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8760 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8107 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9217 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 2740 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1534 |
final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame) {
final var result = new BodyKinematics();
estimateKinematics(timeInterval, frame, oldFrame, result);
return result;
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static BodyKinematics estimateKinematicsAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final double x, final double y, final double z) {
final var result = new BodyKinematics();
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
return result;
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static BodyKinematics estimateKinematicsAndReturnNew(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final double x, final double y, final double z) {
final var result = new BodyKinematics();
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
return result;
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param position cartesian body position resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static BodyKinematics estimateKinematicsAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1621 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1653 |
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates gyroscope calibration parameters containing scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasTurntableGyroscopeCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java | 1397 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 1367 |
PROMedSRobustEasyGyroscopeCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
setupAccelerationFixer();
inliersData = null;
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null
|| qualityScores.length < TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10594 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 11100 |
estimatedMg = preliminaryResult.estimatedMg;
estimatedGg = preliminaryResult.estimatedGg;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
}
/**
* Converts provided NED position expressed in terms of latitude, longitude and height respect
* mean Earth surface, to position expressed in ECEF coordinates.
*
* @param position NED position to be converted.
* @return converted position expressed in ECEF coordinates.
*/
private static ECEFPosition convertPosition(final NEDPosition position) {
final var velocity = new ECEFVelocity();
final var result = new ECEFPosition();
NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
position.getLatitude(), position.getLongitude(), position.getHeight(),
0.0, 0.0, 0.0, result, velocity);
return result;
}
/**
* Converts acceleration instance to meters per squared second.
*
* @param acceleration acceleration instance to be converted.
* @return converted value.
*/
private static double convertAcceleration(final Acceleration acceleration) {
return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Converts angular speed instance to radians per second (rad/s).
*
* @param value angular speed value.
* @param unit unit of angular speed value.
* @return converted value.
*/
private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Converts angular speed instance to radians per second (rad/s).
*
* @param angularSpeed angular speed instance to be converted.
* @return converted value.
*/
private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
}
/**
* Converts time instance to seconds.
*
* @param time time instance to be converted.
* @return converted value.
*/
private static double convertTime(final Time time) {
return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
}
/**
* Internal class containing estimated preliminary result.
*/
protected static class PreliminaryResult {
/**
* Estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*/
private Matrix estimatedMg; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 794 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 801 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 164 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 687 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 170 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 167 |
super(measurements, commonAxisUsed, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 5085 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4350 |
final int i, final double bx, final double by, final double bz,
final double m11, final double m21, final double m31,
final double m12, final double m22, final double m32,
final double m13, final double m23, final double m33,
final double g11, final double g21, final double g31,
final double g12, final double g22, final double g32,
final double g13, final double g23, final double g33) throws EvaluationException {
try {
final var measuredSequence = sequences.get(i);
final var fixedSequence = fixedSequences.get(i);
// generate new sequence using current parameters to fix angular rate measurements
if (measuredSpecificForce == null) {
measuredSpecificForce = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (trueSpecificForce == null) {
trueSpecificForce = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (measuredAngularRate == null) {
measuredAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (trueAngularRate == null) {
trueAngularRate = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (m == null) {
m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (invM == null) {
invM = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (b == null) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 882 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 879 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 884 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5445 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6482 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7386 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8514 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must
* have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5963 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6980 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7863 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8973 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 521 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 519 |
PROMedSRobustKnownFrameMagnetometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
setupWmmEstimator();
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2129 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2125 |
PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this,
progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
initialize();
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 794 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 164 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 801 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 687 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 170 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 167 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 882 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 879 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 884 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/AccelerationFixer.java | 89 |
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 116 |
crossCouplingErrors = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} catch (final WrongSizeException ignore) {
// never happens
}
}
/**
* Gets bias values expressed in meters per squared second (m/s^2).
*
* @return bias values expressed in meters per squared second.
*/
public Matrix getBias() {
return new Matrix(bias);
}
/**
* Gets bias values expressed in meters per squared second (m/s^2).
*
* @param result instance where result will be stored.
*/
public void getBias(final Matrix result) {
bias.copyTo(result);
}
/**
* Sets bias values expressed in meters per squared second (m/s^2).
*
* @param bias bias values expressed in meters per squared second.
* Must be 3x1.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
public void setBias(final Matrix bias) {
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
this.bias = bias;
}
/**
* Gets bias values expressed in meters per squared second (m/s^2).
*
* @return bias values expressed in meters per squared second.
*/
public double[] getBiasArray() {
final var result = new double[BodyKinematics.COMPONENTS];
getBiasArray(result);
return result;
}
/**
* Gets bias values expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
public void getBiasArray(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
try {
bias.toArray(result);
} catch (final WrongSizeException ignore) {
// never happens
}
}
/**
* Sets bias values expressed in meters per squared second (m/s^2).
*
* @param bias bias values expressed in meters per squared second (m/s^2).
* Must have length 3.
* @throws IllegalArgumentException if provided array does not have
* length 3.
*/
public void setBias(final double[] bias) {
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
try {
this.bias.fromArray(bias);
} catch (final WrongSizeException ignore) {
// never happens
}
}
/**
* Gets acceleration bias.
*
* @return acceleration bias.
*/
public AccelerationTriad getBiasAsTriad() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1852 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1755 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1872 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1774 |
PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than the minimum required
* number of samples (10 or 13).
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < getMinimumRequiredMeasurements()) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1571 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1593 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3576 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3641 |
PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null
|| qualityScores.length < TurntableGyroscopeCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1941 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1948 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 188 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 193 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 190 |
final RobustKnownFrameAccelerometerCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java | 601 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 584 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return sequences.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(sequences.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustEasyGyroscopeCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7161 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7308 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7551 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8680 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must be
* 3x1 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8289 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8436 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7640 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7784 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8026 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9136 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias,
initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8748 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8894 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 904 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 908 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 763 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java | 770 |
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
gravityNorm = computeGravityNorm();
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1683 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 517 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1694 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 521 |
PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 5128 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6577 |
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, bx);
b.setElementAtIndex(1, by);
b.setElementAtIndex(2, bz);
g.setElementAt(0, 0, g11);
g.setElementAt(1, 0, g21);
g.setElementAt(2, 0, g31);
g.setElementAt(0, 1, g12);
g.setElementAt(1, 1, g22);
g.setElementAt(2, 1, g32);
g.setElementAt(0, 2, g13);
g.setElementAt(1, 2, g23);
g.setElementAt(2, 2, g33); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3537 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4197 |
if (estimatedMg == null) {
estimatedMg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedMg.initialize(0.0);
}
estimatedMg.setElementAt(0, 0, sx);
estimatedMg.setElementAt(0, 1, mxy);
estimatedMg.setElementAt(1, 1, sy);
estimatedMg.setElementAt(0, 2, mxz);
estimatedMg.setElementAt(1, 2, myz);
estimatedMg.setElementAt(2, 2, sz);
if (estimatedGg == null) {
estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedGg.initialize(0.0);
}
estimatedGg.setElementAtIndex(0, g11);
estimatedGg.setElementAtIndex(1, g21);
estimatedGg.setElementAtIndex(2, g31);
estimatedGg.setElementAtIndex(3, g12);
estimatedGg.setElementAtIndex(4, g22);
estimatedGg.setElementAtIndex(5, g32);
estimatedGg.setElementAtIndex(6, g13);
estimatedGg.setElementAtIndex(7, g23);
estimatedGg.setElementAtIndex(8, g33);
estimatedCovariance = fitter.getCovar();
// propagate covariance matrix so that all parameters are taken into
// account in the order: sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
// g11, g21, g31, g12, g22, g32, g13, g23, g33
// We define a lineal function mapping original parameters for the common
// axis case to the general case
// [sx'] = [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0][sx]
// [sy'] [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0][sy]
// [sz'] [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0][sz]
// [mxy'] [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0][mxy]
// [mxz'] [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0][mxz]
// [myx'] [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0][myz]
// [myz'] [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0][g11]
// [mzx'] [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0][g21]
// [mzy'] [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0][g31]
// [g11'] [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0][g12]
// [g21'] [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0][g22]
// [g31'] [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0][g32]
// [g12'] [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0][g13]
// [g22'] [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0][g23]
// [g32'] [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0][g33]
// [g13'] [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0]
// [g23'] [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0]
// [g33'] [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1]
// As defined in com.irurueta.statistics.MultivariateNormalDist,
// if we consider the jacobian of the lineal application the matrix shown
// above, then covariance can be propagated as follows
final var jacobian = new Matrix(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
for (var i = 0; i < 5; i++) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2123 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2120 |
PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.this,
progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS_COMMON_Z_AXIS) {
throw new IllegalArgumentException();
}
this.qualityScores = qualityScores;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 814 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 185 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 823 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 597 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 581 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 190 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 187 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 902 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 900 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 905 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1593 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1571 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1690 |
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 1645 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java | 1678 |
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates gyroscope calibration parameters containing scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownBiasTurntableGyroscopeCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1941 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1940 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1944 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 876 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 984 |
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var fMeasX = measuredKinematics.getFx();
final var fMeasY = measuredKinematics.getFy();
final var fMeasZ = measuredKinematics.getFz();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, 1.0);
a.setElementAt(i, 3, fTrueX);
a.setElementAt(i, 6, fTrueY);
a.setElementAt(i, 7, fTrueZ);
b.setElementAtIndex(i, fMeasX - fTrueX);
i++;
a.setElementAt(i, 1, 1.0);
a.setElementAt(i, 4, fTrueY);
a.setElementAt(i, 8, fTrueZ); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 193 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 818 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 826 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 712 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 194 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 191 |
super(measurements, commonAxisUsed, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5216 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6253 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must
* have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7634 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8599 |
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5735 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6752 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8110 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9059 |
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 906 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 904 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 908 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 1421 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 779 |
return estimateKinematicsAndReturnNew(timeInterval, c, oldC, velocity, oldVelocity, position);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final Point3D position) {
return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final Point3D position) {
return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates)..
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF-frame coordinate transformation.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx x coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
* along ECEF-frame axes.
* @param vy y coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
* along ECEF-frame axes.
* @param vz z coordinate of velocity of body frame expressed in meters per second (m/s) and resolved
* along ECEF-frame axes.
* @param oldVx x coordinate of previous velocity of body frame expressed in meters per second (m/s) and
* resolved along ECEF-frame axes.
* @param oldVy y coordinate of previous velocity of body frame expressed in meters per second (m/s) and
* resolved along ECEF-frame axes.
* @param oldVz z coordinate of previous velocity of body frame expressed in meters per second (m/s) and
* resolved along ECEF-frame axes.
* @param x cartesian x coordinate of body position expressed in meters (m) and resolved along
* ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m) and resolved along
* ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m) and resolved along
* ECEF-frame axes.
* @param result instance where estimated body kinematics will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz,
final double x, final double y, final double z, final BodyKinematics result) {
if (timeInterval < 0.0 || !ECEFFrame.isValidCoordinateTransformation(c) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/AccelerometerNonLinearCalibrator.java | 24 |
| com/irurueta/navigation/inertial/calibration/gyroscope/GyroscopeNonLinearCalibrator.java | 24 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MagnetometerNonLinearCalibrator.java | 24 |
public interface AccelerometerNonLinearCalibrator extends AccelerometerCalibrator {
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
double getInitialSx();
/**
* Sets initial x scaling factor.
*
* @param initialSx initial x scaling factor.
* @throws LockedException if calibrator is currently running.
*/
void setInitialSx(final double initialSx) throws LockedException;
/**
* Gets initial y scaling factor.
*
* @return initial y scaling factor.
*/
double getInitialSy();
/**
* Sets initial y scaling factor.
*
* @param initialSy initial y scaling factor.
* @throws LockedException if calibrator is currently running.
*/
void setInitialSy(final double initialSy) throws LockedException;
/**
* Gets initial z scaling factor.
*
* @return initial z scaling factor.
*/
double getInitialSz();
/**
* Sets initial z scaling factor.
*
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
void setInitialSz(final double initialSz) throws LockedException;
/**
* Gets initial x-y cross coupling error.
*
* @return initial x-y cross coupling error.
*/
double getInitialMxy();
/**
* Sets initial x-y cross coupling error.
*
* @param initialMxy initial x-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
void setInitialMxy(final double initialMxy) throws LockedException;
/**
* Gets initial x-z cross coupling error.
*
* @return initial x-z cross coupling error.
*/
double getInitialMxz();
/**
* Sets initial x-z cross coupling error.
*
* @param initialMxz initial x-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
void setInitialMxz(final double initialMxz) throws LockedException;
/**
* Gets initial y-x cross coupling error.
*
* @return initial y-x cross coupling error.
*/
double getInitialMyx();
/**
* Sets initial y-x cross coupling error.
*
* @param initialMyx initial y-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
void setInitialMyx(final double initialMyx) throws LockedException;
/**
* Gets initial y-z cross coupling error.
*
* @return initial y-z cross coupling error.
*/
double getInitialMyz();
/**
* Sets initial y-z cross coupling error.
*
* @param initialMyz initial y-z cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
void setInitialMyz(final double initialMyz) throws LockedException;
/**
* Gets initial z-x cross coupling error.
*
* @return initial z-x cross coupling error.
*/
double getInitialMzx();
/**
* Sets initial z-x cross coupling error.
*
* @param initialMzx initial z-x cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
void setInitialMzx(final double initialMzx) throws LockedException;
/**
* Gets initial z-y cross coupling error.
*
* @return initial z-y cross coupling error.
*/
double getInitialMzy();
/**
* Sets initial z-y cross coupling error.
*
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
void setInitialMzy(final double initialMzy) throws LockedException;
/**
* Sets initial scaling factors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @throws LockedException if calibrator is currently running.
*/
void setInitialScalingFactors(final double initialSx, final double initialSy, final double initialSz)
throws LockedException;
/**
* Sets initial cross coupling errors.
*
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
void setInitialCrossCouplingErrors(
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException;
/**
* Sets initial scaling factors and cross coupling errors.
*
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @throws LockedException if calibrator is currently running.
*/
void setInitialScalingFactorsAndCrossCouplingErrors(
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) throws LockedException;
/**
* Gets initial scale factors and cross coupling errors matrix.
*
* @return initial scale factors and cross coupling errors matrix.
*/
Matrix getInitialMa(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1507 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1571 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 336 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1593 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1511 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3396 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3459 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 339 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 342 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1941 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1948 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1940 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1944 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1704 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1869 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1773 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 535 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1889 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1795 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1710 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3595 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 539 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3658 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2141 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2138 |
PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 193 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 910 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 818 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 826 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 712 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 194 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 191 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 906 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 904 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 908 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 595 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 321 |
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
estimateKinematics(timeInterval, frame, oldC, oldVx, oldVy, oldVz, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz, final Point3D position,
final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz, final Point3D position,
final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param velocity velocity of body frame with respect ECEF frame.
* @param oldVelocity previous velocity of body frame with respect ECEF frame.
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 6360 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 6083 |
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
innerCalibrator.setMeasurements(meas);
innerCalibrator.calibrate();
innerCalibrator.getEstimatedBiases(result.estimatedBiases);
result.estimatedMa = innerCalibrator.getEstimatedMa();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierMeasurements.add(measurements.get(i));
}
}
try {
innerCalibrator.setInitialBias(preliminaryResult.estimatedBiases);
innerCalibrator.setInitialMa(preliminaryResult.estimatedMa);
innerCalibrator.setCommonAxisUsed(commonAxisUsed);
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3830 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3790 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3842 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3877 |
public void setMeasurements(final Collection<StandardDeviationBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @return position where body kinematics measures have been taken.
*/
public ECEFPosition getEcefPosition() {
return position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final ECEFPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @return position where body kinematics measures have been taken or null if
* not available.
*/
public NEDPosition getNedPosition() {
final var result = new NEDPosition();
return getNedPosition(result) ? result : null;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param result instance where result will be stored.
* @return true if NED position could be computed, false otherwise.
*/
public boolean getNedPosition(final NEDPosition result) {
if (position != null) {
final var velocity = new NEDVelocity();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
position.getX(), position.getY(), position.getZ(), 0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final NEDPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = convertPosition(position);
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.STANDARD_DEVIATION_BODY_KINEMATICS_MEASUREMENT;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return false; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/StandardDeviationFrameBodyKinematics.java | 1261 |
| com/irurueta/navigation/inertial/calibration/StandardDeviationTimedBodyKinematics.java | 345 |
public StandardDeviationFrameBodyKinematics(final StandardDeviationFrameBodyKinematics input) {
copyFrom(input);
}
/**
* Gets standard deviation of measured specific force expressed in meters per squared
* second (m/s^2).
*
* @return standard deviation of measured specific force.
*/
public double getSpecificForceStandardDeviation() {
return specificForceStandardDeviation;
}
/**
* Sets standard deviation of measured specific force expressed in meters per squared
* second (m/s^2).
*
* @param specificForceStandardDeviation standard deviation of measured specific force.
* @throws IllegalArgumentException if provided value is negative.
*/
public void setSpecificForceStandardDeviation(final double specificForceStandardDeviation) {
if (specificForceStandardDeviation < 0.0) {
throw new IllegalArgumentException();
}
this.specificForceStandardDeviation = specificForceStandardDeviation;
}
/**
* Gets standard deviation of measured specific force.
*
* @return standard deviation of measured specific force.
*/
public Acceleration getSpecificForceStandardDeviationAsAcceleration() {
return new Acceleration(specificForceStandardDeviation, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets standard deviation of measured specific force.
*
* @param result instance where standard deviation of measured specific force will be
* stored.
*/
public void getSpecificForceStandardDeviationAsAcceleration(final Acceleration result) {
result.setValue(specificForceStandardDeviation);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets standard deviation of measured specific force.
*
* @param specificForceStandardDeviation standard deviation of measured specific force.
* @throws IllegalArgumentException if provided value is negative.
*/
public void setSpecificForceStandardDeviation(final Acceleration specificForceStandardDeviation) {
setSpecificForceStandardDeviation(convertAcceleration(specificForceStandardDeviation));
}
/**
* Gets standard deviation of measured angular rate expressed in radians per second (rad/s).
*
* @return standard deviation of measured angular rate.
*/
public double getAngularRateStandardDeviation() {
return angularRateStandardDeviation;
}
/**
* Sets standard deviation of measured angular rate expressed in radians per second (rad/s).
*
* @param angularRateStandardDeviation standard deviation of measured angular rate.
* @throws IllegalArgumentException if provided value is negative.
*/
public void setAngularRateStandardDeviation(final double angularRateStandardDeviation) {
if (angularRateStandardDeviation < 0.0) {
throw new IllegalArgumentException();
}
this.angularRateStandardDeviation = angularRateStandardDeviation;
}
/**
* Gets standard deviation of measured angular rate.
*
* @return standard deviation of measured angular rate.
*/
public AngularSpeed getAngularRateStandardDeviationAsAngularSpeed() {
return new AngularSpeed(angularRateStandardDeviation, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets standard deviation of measured angular rate.
*
* @param result instance where standard deviation of measured angular rate will be
* stored.
*/
public void getAngularRateStandardDeviationAsAngularSpeed(final AngularSpeed result) {
result.setValue(angularRateStandardDeviation);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets standard deviation of measured angular rate.
*
* @param angularRateStandardDeviation standard deviation of measured angular rate.
* @throws IllegalArgumentException if provided value is negative.
*/
public void setAngularRateStandardDeviation(final AngularSpeed angularRateStandardDeviation) {
setAngularRateStandardDeviation(convertAngularSpeed(angularRateStandardDeviation));
}
/**
* Copies data of provided instance into this instance.
*
* @param input instance to copy data from.
*/
public void copyFrom(final StandardDeviationFrameBodyKinematics input) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5006 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5143 |
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5370 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6407 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must be
* 3x1 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6043 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6180 |
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6879 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8007 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5527 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5661 |
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5887 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6904 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6544 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6678 |
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7366 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8474 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 54 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 46 |
private static final double SCALING_THRESHOLD = 2e-5;
/**
* Alpha threshold.
*/
private static final double ALPHA_THRESHOLD = 1e-8;
/**
* Number of rows.
*/
private static final int ROWS = 3;
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz,
final double x, final double y, final double z, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz,
final double x, final double y, final double z, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param velocity velocity of body frame with respect ECEF frame.
* @param oldVelocity previous velocity of body frame with respect ECEF frame.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7306 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8434 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7389 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7554 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must
* have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8517 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8683 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7782 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8892 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7866 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8029 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8976 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9139 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 907 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 1048 |
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var omegaMeasX = measuredKinematics.getAngularRateX();
final var omegaMeasY = measuredKinematics.getAngularRateY();
final var omegaMeasZ = measuredKinematics.getAngularRateZ();
final var omegaTrueX = expectedKinematics.getAngularRateX();
final var omegaTrueY = expectedKinematics.getAngularRateY();
final var omegaTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, 1.0);
a.setElementAt(i, 3, omegaTrueX);
a.setElementAt(i, 6, omegaTrueY);
a.setElementAt(i, 7, omegaTrueZ);
a.setElementAt(i, 9, fTrueX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/generators/AccelerometerAndGyroscopeMeasurementsGenerator.java | 564 |
| com/irurueta/navigation/inertial/calibration/generators/AccelerometerGyroscopeAndMagnetometerMeasurementsGenerator.java | 650 |
gyroscopeMeasurementsGenerator.setBaseNoiseLevelAbsoluteThreshold(baseNoiseLevelAbsoluteThreshold);
}
/**
* Gets internal status of this generator.
*
* @return internal status of this generator.
*/
public TriadStaticIntervalDetector.Status getStatus() {
return accelerometerMeasurementsGenerator.getStatus();
}
/**
* Gets accelerometer base noise level that has been detected during
* initialization expressed in meters per squared second (m/s^2).
* This is equal to the standard deviation of the accelerometer measurements
* during initialization phase.
*
* @return accelerometer base noise level.
*/
public double getAccelerometerBaseNoiseLevel() {
return accelerometerMeasurementsGenerator.getAccelerometerBaseNoiseLevel();
}
/**
* Gets accelerometer base noise level that has been detected during
* initialization.
* This is equal to the standard deviation of the accelerometer measurements
* during initialization phase.
*
* @return measurement base noise level.
*/
public Acceleration getAccelerometerBaseNoiseLevelAsMeasurement() {
return accelerometerMeasurementsGenerator.getAccelerometerBaseNoiseLevelAsMeasurement();
}
/**
* Gets accelerometer base noise level that has been detected during
* initialization.
*
* @param result instance where result will be stored.
*/
public void getAccelerometerBaseNoiseLevelAsMeasurement(final Acceleration result) {
accelerometerMeasurementsGenerator.getAccelerometerBaseNoiseLevelAsMeasurement(result);
}
/**
* Gets accelerometer base noise level PSD (Power Spectral Density)
* expressed in (m^2 * s^-3).
*
* @return accelerometer base noise level PSD.
*/
public double getAccelerometerBaseNoiseLevelPsd() {
return accelerometerMeasurementsGenerator.getAccelerometerBaseNoiseLevelPsd();
}
/**
* Gets accelerometer base noise level root PSD (Power Spectral Density)
* expressed in (m * s^-1.5).
*
* @return accelerometer base noise level root PSD.
*/
@Override
public double getAccelerometerBaseNoiseLevelRootPsd() {
return accelerometerMeasurementsGenerator.getAccelerometerBaseNoiseLevelRootPsd();
}
/**
* Gets threshold to determine static/dynamic period changes expressed in
* meters per squared second (m/s^2).
*
* @return threshold to determine static/dynamic period changes.
*/
public double getThreshold() {
return accelerometerMeasurementsGenerator.getThreshold();
}
/**
* Gets threshold to determine static/dynamic period changes.
*
* @return threshold to determine static/dynamic period changes.
*/
public Acceleration getThresholdAsMeasurement() {
return accelerometerMeasurementsGenerator.getThresholdAsMeasurement();
}
/**
* Gets threshold to determine static/dynamic period changes.
*
* @param result instance where result will be stored.
*/
public void getThresholdAsMeasurement(final Acceleration result) {
accelerometerMeasurementsGenerator.getThresholdAsMeasurement(result);
}
/**
* Gets number of samples that have been processed in a static period so far.
*
* @return number of samples that have been processed in a static period so far.
*/
public int getProcessedStaticSamples() {
return accelerometerMeasurementsGenerator.getProcessedStaticSamples();
}
/**
* Gets number of samples that have been processed in a dynamic period so far.
*
* @return number of samples that have been processed in a dynamic period so far.
*/
public int getProcessedDynamicSamples() {
return accelerometerMeasurementsGenerator.getProcessedDynamicSamples();
}
/**
* Indicates whether last static interval must be skipped.
*
* @return true if last static interval must be skipped.
*/
public boolean isStaticIntervalSkipped() {
return accelerometerMeasurementsGenerator.isStaticIntervalSkipped();
}
/**
* Indicates whether last dynamic interval must be skipped.
*
* @return true if last dynamic interval must be skipped.
*/
public boolean isDynamicIntervalSkipped() {
return accelerometerMeasurementsGenerator.isDynamicIntervalSkipped();
}
/**
* Indicates whether generator is running or not.
*
* @return true if generator is running, false otherwise.
*/
public boolean isRunning() {
return running;
}
/**
* Processes a sample of data.
*
* @param sample sample of data to be processed.
* @return true if provided samples has been processed, false if provided triad has been skipped because
* generator previously failed. If generator previously failed, it will need to be reset before
* processing additional samples.
* @throws LockedException if generator is busy processing a previous sample.
*/
public boolean process(final TimedBodyKinematics sample) throws LockedException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7015 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8143 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7159 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8287 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7497 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8605 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7638 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8746 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 420 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 206 |
final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame, final BodyKinematics result) {
estimateKinematics(timeInterval, frame, oldFrame, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final double x, final double y, final double z, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final double x, final double y, final double z, final BodyKinematics result) {
estimateKinematics(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param position cartesian body position resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public void estimate(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 5817 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 5567 |
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
innerCalibrator.setMeasurements(meas);
innerCalibrator.calibrate();
result.estimatedMa = innerCalibrator.getEstimatedMa();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierMeasurements.add(measurements.get(i));
}
}
try {
innerCalibrator.setBiasCoordinates(biasX, biasY, biasZ);
innerCalibrator.setInitialMa(preliminaryResult.estimatedMa);
innerCalibrator.setCommonAxisUsed(commonAxisUsed);
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 906 |
public void setThreshold(double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates magnetometer calibration parameters containing soft-iron
* scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5448 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6332 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5966 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6833 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/StandardDeviationBodyKinematics.java | 173 |
| com/irurueta/navigation/inertial/calibration/StandardDeviationFrameBodyKinematics.java | 1263 |
| com/irurueta/navigation/inertial/calibration/StandardDeviationTimedBodyKinematics.java | 347 |
}
/**
* Gets standard deviation of measured specific force expressed in meters per squared
* second (m/s^2).
*
* @return standard deviation of measured specific force.
*/
public double getSpecificForceStandardDeviation() {
return specificForceStandardDeviation;
}
/**
* Sets standard deviation of measured specific force expressed in meters per squared
* second (m/s^2).
*
* @param specificForceStandardDeviation standard deviation of measured specific force.
* @throws IllegalArgumentException if provided value is negative.
*/
public void setSpecificForceStandardDeviation(final double specificForceStandardDeviation) {
if (specificForceStandardDeviation < 0.0) {
throw new IllegalArgumentException();
}
this.specificForceStandardDeviation = specificForceStandardDeviation;
}
/**
* Gets standard deviation of measured specific force.
*
* @return standard deviation of measured specific force.
*/
public Acceleration getSpecificForceStandardDeviationAsAcceleration() {
return new Acceleration(specificForceStandardDeviation, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Gets standard deviation of measured specific force.
*
* @param result instance where standard deviation of measured specific force will be
* stored.
*/
public void getSpecificForceStandardDeviationAsAcceleration(final Acceleration result) {
result.setValue(specificForceStandardDeviation);
result.setUnit(AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets standard deviation of measured specific force.
*
* @param specificForceStandardDeviation standard deviation of measured specific force.
* @throws IllegalArgumentException if provided value is negative.
*/
public void setSpecificForceStandardDeviation(final Acceleration specificForceStandardDeviation) {
setSpecificForceStandardDeviation(convertAcceleration(specificForceStandardDeviation));
}
/**
* Gets standard deviation of measured angular rate expressed in radians per second (rad/s).
*
* @return standard deviation of measured angular rate.
*/
public double getAngularRateStandardDeviation() {
return angularRateStandardDeviation;
}
/**
* Sets standard deviation of measured angular rate expressed in radians per second (rad/s).
*
* @param angularRateStandardDeviation standard deviation of measured angular rate.
* @throws IllegalArgumentException if provided value is negative.
*/
public void setAngularRateStandardDeviation(final double angularRateStandardDeviation) {
if (angularRateStandardDeviation < 0.0) {
throw new IllegalArgumentException();
}
this.angularRateStandardDeviation = angularRateStandardDeviation;
}
/**
* Gets standard deviation of measured angular rate.
*
* @return standard deviation of measured angular rate.
*/
public AngularSpeed getAngularRateStandardDeviationAsAngularSpeed() {
return new AngularSpeed(angularRateStandardDeviation, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Gets standard deviation of measured angular rate.
*
* @param result instance where standard deviation of measured angular rate will be
* stored.
*/
public void getAngularRateStandardDeviationAsAngularSpeed(final AngularSpeed result) {
result.setValue(angularRateStandardDeviation);
result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets standard deviation of measured angular rate.
*
* @param angularRateStandardDeviation standard deviation of measured angular rate.
* @throws IllegalArgumentException if provided value is negative.
*/
public void setAngularRateStandardDeviation(final AngularSpeed angularRateStandardDeviation) {
setAngularRateStandardDeviation(convertAngularSpeed(angularRateStandardDeviation));
}
/**
* Copies data of provided instance into this instance.
*
* @param input instance to copy data from.
*/
public void copyFrom(final StandardDeviationBodyKinematics input) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1683 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1852 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1755 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 517 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1872 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1774 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1694 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3576 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 521 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3641 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2123 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2120 |
PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4833 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6273 |
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
}
/**
* Makes proper conversion of internal cross-coupling, bias and g-dependent
* cross bias matrices.
*
* @param m internal scaling and cross-coupling matrix.
* @param b internal bias matrix.
* @param g internal g-dependent cross bias matrix.
* @throws AlgebraException if a numerical instability occurs.
*/
private void setResult(final Matrix m, final Matrix b, final Matrix g) throws AlgebraException {
setResult(m, b);
// Gg = M*G
m.multiply(g, estimatedGg);
}
/**
* Makes proper conversion of internal cross-coupling and bias matrices.
*
* @param m internal scaling and cross-coupling matrix.
* @param b internal bias matrix.
* @throws AlgebraException if a numerical instability occurs.
*/
private void setResult(final Matrix m, final Matrix b) throws AlgebraException {
// Because:
// M = I + Mg
// b = M^-1*bg
// Then:
// Mg = M - I
// bg = M*b
if (estimatedBiases == null) {
estimatedBiases = new double[BodyKinematics.COMPONENTS];
}
final var bg = m.multiplyAndReturnNew(b);
bg.toArray(estimatedBiases);
if (estimatedMg == null) {
estimatedMg = m;
} else {
estimatedMg.copyFrom(m);
}
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
estimatedMg.setElementAt(i, i, estimatedMg.getElementAt(i, i) - 1.0);
}
if (estimatedGg == null) {
estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedGg.initialize(0.0);
}
estimatedCovariance = fitter.getCovar();
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Computes gravity versor error at the end of a sequence using provided
* parameters.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param i row position.
* @param params array containing parameters for the general purpose case
* when G-dependent cross biases are taken into account. Must
* have length 18.
* @return error between estimated and measured gravity versor.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 343 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 343 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 345 |
final boolean commonAxisUsed, final RobustKnownFrameGyroscopeCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(
new PROMedSRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 537 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3621 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3585 |
public void setListener(final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the estimator.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or no.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Gets Earth's magnetic model.
*
* @return Earth's magnetic model or null if not provided.
*/
public WorldMagneticModel getMagneticModel() {
return magneticModel;
}
/**
* Sets Earth's magnetic model.
* If not provided a default model will be loaded internally.
*
* @param magneticModel Earth's magnetic model to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
if (running) {
throw new LockedException();
}
this.magneticModel = magneticModel;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | IOException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1402 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 929 |
final var myz = unknowns.getElementAtIndex(5);
fillMa(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
}
/**
* Internal method to perform general calibration.
*
* @throws AlgebraException if there are numerical errors.
*/
private void calibrateGeneral() throws AlgebraException {
// The accelerometer model is:
// fmeas = ba + (I + Ma) * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// fmeas = ba + (I + Ma) * ftrue
// Hence:
// [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
// [fmeasy] = [by] [0 1 0] [myx sy myz] [ftruey]
// [fmeasz] = [bz] [0 0 1] [mzx mzy sz ] [ftruez]
// [fmeasx] = [bx] + [1+sx mxy mxz ][ftruex]
// [fmeasy] [by] [myx 1+sy myz ][ftruey]
// [fmeasz] [bz] [mzx mzy 1+sz][ftruez]
// fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + (1+sy) * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + (1+sz) * ftruez
// Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
// Reordering:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez
// fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy - ftruey - by = myx * ftruex + sy * ftruey + myz * ftruez
// fmeasz - ftruez - bz = mzx * ftruex + mzy * ftruey + sz * ftruez
// [ftruex 0 0 ftruey ftruez 0 0 0 0 ][sx ] = [fmeasx - ftruex - bx]
// [0 ftruey 0 0 0 ftruex ftruez 0 0 ][sy ] [fmeasy - ftruey - by]
// [0 0 ftruez 0 0 0 0 ftruex ftruey][sz ] [fmeasz - ftruez - bz]
// [mxy]
// [mxz]
// [myx]
// [myz]
// [mzx]
// [mzy]
final var expectedKinematics = new BodyKinematics();
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var fMeasX = measuredKinematics.getFx();
final var fMeasY = measuredKinematics.getFy();
final var fMeasZ = measuredKinematics.getFz();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, fTrueX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 703 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 707 |
final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1509 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1517 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4746 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5782 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5280 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6297 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 184 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 186 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 188 |
final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5478 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10599 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 11105 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6216 |
}
/**
* Converts provided NED position expressed in terms of latitude, longitude and height respect
* mean Earth surface, to position expressed in ECEF coordinates.
*
* @param position NED position to be converted.
* @return converted position expressed in ECEF coordinates.
*/
private static ECEFPosition convertPosition(final NEDPosition position) {
final var velocity = new ECEFVelocity();
final var result = new ECEFPosition();
NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
position.getLatitude(), position.getLongitude(), position.getHeight(), 0.0, 0.0, 0.0,
result, velocity);
return result;
}
/**
* Converts acceleration instance to meters per squared second.
*
* @param acceleration acceleration instance to be converted.
* @return converted value.
*/
private static double convertAcceleration(final Acceleration acceleration) {
return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Converts angular speed instance to radians per second (rad/s).
*
* @param value angular speed value.
* @param unit unit of angular speed value.
* @return converted value.
*/
private static double convertAngularSpeed(final double value, final AngularSpeedUnit unit) {
return AngularSpeedConverter.convert(value, unit, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Converts angular speed instance to radians per second (rad/s).
*
* @param angularSpeed angular speed instance to be converted.
* @return converted value.
*/
private static double convertAngularSpeed(final AngularSpeed angularSpeed) {
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
}
/**
* Converts time instance to seconds.
*
* @param time time instance to be converted.
* @return converted value.
*/
private static double convertTime(final Time time) {
return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 902 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 909 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 750 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 735 |
listener.onCalibrateProgressChange(RANSACRobustEasyGyroscopeCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
setupAccelerationFixer();
inliersData = null;
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.RANSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5141 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6178 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5219 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5373 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must
* have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6256 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6410 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must
* have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5659 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6676 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5738 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5890 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6755 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6907 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 342 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 341 |
RANSACRobustKnownFrameMagnetometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
setupWmmEstimator();
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.RANSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1061 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1060 |
RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
initialize();
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.RANSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 2860 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 2352 |
final PreliminaryResult preliminaryResult) {
// The magnetometer model is:
// mBmeas = ba + (I + Mm) * mBtrue
// Hence:
// [mBmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [mBtruex]
// [mBmeasy] = [by] [0 1 0] [myx sy myz] [mBtruey]
// [mBmeasz] = [bz] [0 0 1] [mzx mzy sz ] [mBtruez]
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
final var ecefFrame = measurement.getFrame();
final var nedFrame = ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(ecefFrame);
final var year = measurement.getYear();
final var latitude = nedFrame.getLatitude();
final var longitude = nedFrame.getLongitude();
final var height = nedFrame.getHeight();
final var earthB = wmmEstimator.estimate(latitude, longitude, height, year);
final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
nedFrame.getCoordinateTransformation(cbn);
cbn.inverse(cnb);
final var expectedMagneticFluxDensity = BodyMagneticFluxDensityEstimator.estimate(earthB, cnb);
final var bMeasX1 = measuredMagneticFluxDensity.getBx();
final var bMeasY1 = measuredMagneticFluxDensity.getBy();
final var bMeasZ1 = measuredMagneticFluxDensity.getBz();
final var bTrueX = expectedMagneticFluxDensity.getBx();
final var bTrueY = expectedMagneticFluxDensity.getBy();
final var bTrueZ = expectedMagneticFluxDensity.getBz(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 339 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 343 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 343 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 345 |
final boolean commonAxisUsed, final RobustKnownFrameAccelerometerCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<PreliminaryResult>( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1369 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1500 |
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var omegaMeasX = measuredKinematics.getAngularRateX();
final var omegaMeasY = measuredKinematics.getAngularRateY();
final var omegaMeasZ = measuredKinematics.getAngularRateZ();
final var omegaTrueX = expectedKinematics.getAngularRateX();
final var omegaTrueY = expectedKinematics.getAngularRateY();
final var omegaTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, omegaTrueX);
a.setElementAt(i, 3, omegaTrueY);
a.setElementAt(i, 4, omegaTrueZ);
a.setElementAt(i, 6, fTrueX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10515 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 11018 |
innerCalibrator.calibrate();
result.estimatedMg = innerCalibrator.getEstimatedMg();
result.estimatedGg = innerCalibrator.getEstimatedGg();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierMeasurements.add(measurements.get(i));
}
}
try {
innerCalibrator.setTurntableRotationRate(turntableRotationRate);
innerCalibrator.setTimeInterval(timeInterval);
innerCalibrator.setGDependentCrossBiasesEstimated(estimateGDependentCrossBiases);
innerCalibrator.setInitialMg(preliminaryResult.estimatedMg); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 1305 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 715 |
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
final Speed oldVx, final Speed oldVy, final Speed oldVz) {
return estimateKinematicsAndReturnNew(timeInterval, frame, oldC, oldVx, oldVy, oldVz);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz, final Point3D position) {
return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, position);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param velocity velocity of body frame with respect ECEF frame.
* @param oldVelocity previous velocity of body frame with respect ECEF frame.
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final ECEFVelocity velocity, final ECEFVelocity oldVelocity, final Point3D position) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1509 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 344 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 344 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1517 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 346 |
super(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(
new PROMedSRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4871 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5908 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5004 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6041 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6811 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7939 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5399 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6416 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5525 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6542 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7301 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8409 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3967 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4715 |
initial[3] = initialSx;
initial[4] = initialSy;
initial[5] = initialSz;
initial[6] = initialMxy;
initial[7] = initialMxz;
initial[8] = initialMyx;
initial[9] = initialMyz;
initial[10] = initialMzx;
initial[11] = initialMzy;
return initial;
}
@Override
public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
final Matrix jacobian) {
// We know that:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez
// Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
// sz, mxy, mxz, myx, myz, mzx and mzy is:
// d(fmeasx)/d(bx) = 1.0
// d(fmeasx)/d(by) = 0.0
// d(fmeasx)/d(bz) = 0.0
// d(fmeasx)/d(sx) = ftruex
// d(fmeasx)/d(sy) = 0.0
// d(fmeasx)/d(sz) = 0.0
// d(fmeasx)/d(mxy) = ftruey
// d(fmeasx)/d(mxz) = ftruez
// d(fmeasx)/d(myx) = 0.0
// d(fmeasx)/d(myz) = 0.0
// d(fmeasx)/d(mzx) = 0.0
// d(fmeasx)/d(mzy) = 0.0
// d(fmeasy)/d(bx) = 0.0
// d(fmeasy)/d(by) = 1.0
// d(fmeasy)/d(bz) = 0.0
// d(fmeasy)/d(sx) = 0.0
// d(fmeasy)/d(sy) = ftruey
// d(fmeasy)/d(sz) = 0.0
// d(fmeasy)/d(mxy) = 0.0
// d(fmeasy)/d(mxz) = 0.0
// d(fmeasy)/d(myx) = ftruex
// d(fmeasy)/d(myz) = ftruez
// d(fmeasy)/d(mzx) = 0.0
// d(fmeasy)/d(mzy) = 0.0
// d(fmeasz)/d(bx) = 0.0
// d(fmeasz)/d(by) = 0.0
// d(fmeasz)/d(bz) = 1.0
// d(fmeasz)/d(sx) = 0.0
// d(fmeasz)/d(sy) = 0.0
// d(fmeasz)/d(sz) = ftruez
// d(fmeasz)/d(mxy) = 0.0
// d(fmeasz)/d(mxz) = 0.0
// d(fmeasz)/d(myx) = 0.0
// d(fmeasz)/d(myz) = 0.0
// d(fmeasz)/d(mzx) = ftruex
// d(fmeasz)/d(mzy) = ftruey
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var sx = params[3];
final var sy = params[4];
final var sz = params[5];
final var mxy = params[6];
final var mxz = params[7];
final var myx = params[8];
final var myz = params[9];
final var mzx = params[10];
final var mzy = params[11];
final var ftruex = point[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7470 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7634 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7949 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8110 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1345 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1455 |
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var fMeasX = measuredKinematics.getFx();
final var fMeasY = measuredKinematics.getFy();
final var fMeasZ = measuredKinematics.getFz();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, fTrueX);
a.setElementAt(i, 1, 0.0);
a.setElementAt(i, 2, 0.0);
a.setElementAt(i, 3, fTrueY);
a.setElementAt(i, 4, fTrueZ);
a.setElementAt(i, 5, 0.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2681 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2903 |
public void setListener(final KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1509 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 346 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1944 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6881 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7017 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7232 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8360 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8009 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8145 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7369 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7499 |
final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7710 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8820 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8477 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8607 |
final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3204 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4134 |
return BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[GENERAL_UNKNOWNS];
initial[0] = initialSx;
initial[1] = initialSy;
initial[2] = initialSz;
initial[3] = initialMxy;
initial[4] = initialMxz;
initial[5] = initialMyx;
initial[6] = initialMyz;
initial[7] = initialMzx;
initial[8] = initialMzy;
return initial;
}
@Override
public void evaluate(
final int i, final double[] point, final double[] result,
final double[] params, final Matrix jacobian) {
// We know that:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez
// Hence, the derivatives respect the parameters sx, sy, sz,
// mxy, mxz, myx, myz, mzx and mzy is:
// d(fmeasx)/d(sx) = ftruex
// d(fmeasx)/d(sy) = 0.0
// d(fmeasx)/d(sz) = 0.0
// d(fmeasx)/d(mxy) = ftruey
// d(fmeasx)/d(mxz) = ftruez
// d(fmeasx)/d(myx) = 0.0
// d(fmeasx)/d(myz) = 0.0
// d(fmeasx)/d(mzx) = 0.0
// d(fmeasx)/d(mzy) = 0.0
// d(fmeasy)/d(sx) = 0.0
// d(fmeasy)/d(sy) = ftruey
// d(fmeasy)/d(sz) = 0.0
// d(fmeasy)/d(mxy) = 0.0
// d(fmeasy)/d(mxz) = 0.0
// d(fmeasy)/d(myx) = ftruex
// d(fmeasy)/d(myz) = ftruez
// d(fmeasy)/d(mzx) = 0.0
// d(fmeasy)/d(mzy) = 0.0
// d(fmeasz)/d(sx) = 0.0
// d(fmeasz)/d(sy) = 0.0
// d(fmeasz)/d(sz) = ftruez
// d(fmeasz)/d(mxy) = 0.0
// d(fmeasz)/d(mxz) = 0.0
// d(fmeasz)/d(myx) = 0.0
// d(fmeasz)/d(myz) = 0.0
// d(fmeasz)/d(mzx) = ftruex
// d(fmeasz)/d(mzy) = ftruey
final var sx = params[0];
final var sy = params[1];
final var sz = params[2];
final var mxy = params[3];
final var mxz = params[4];
final var myx = params[5];
final var myz = params[6];
final var mzx = params[7];
final var mzy = params[8];
final var ftruex = point[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 704 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 185 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 190 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 187 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 857 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 969 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 916 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 978 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 925 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 863 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1796 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 345 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1828 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1058 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1055 |
RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.RANSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3963 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5313 |
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var m11 = result[3];
final var m21 = result[4];
final var m31 = result[5];
final var m12 = result[6];
final var m22 = result[7];
final var m32 = result[8];
final var m13 = result[9];
final var m23 = result[10];
final var m33 = result[11];
final var g11 = result[12];
final var g21 = result[13];
final var g31 = result[14];
final var g12 = result[15];
final var g22 = result[16];
final var g32 = result[17];
final var g13 = result[18];
final var g23 = result[19];
final var g33 = result[20];
final var b = new Matrix(BodyKinematics.COMPONENTS, 1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 189 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 909 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 900 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 905 |
super(position, measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates magnetometer calibration parameters containing soft-iron
* scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6631 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6748 |
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final double[] bias, final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6947 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8075 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7087 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8215 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events
* raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7089 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7234 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events
* raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7759 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7877 |
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8217 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8362 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events
* raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7124 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7240 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7432 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8540 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7567 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8675 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7569 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7712 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8232 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8348 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8677 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8822 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1058 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2276 |
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
// estimate Earth magnetic flux density at frame position and
// timestamp using WMM
final var ecefFrame = measurement.getFrame();
ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);
final var year = measurement.getYear();
final var latitude = nedFrame.getLatitude();
final var longitude = nedFrame.getLongitude();
final var height = nedFrame.getHeight();
nedFrame.getCoordinateTransformation(cbn);
cbn.inverse(cnb);
wmmEstimator.estimate(latitude, longitude, height, year, earthB);
// estimate expected body magnetic flux density taking into
// account body attitude (inverse of frame orientation) and
// estimated Earth magnetic flux density
BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);
final var bMeasX = measuredMagneticFluxDensity.getBx();
final var bMeasY = measuredMagneticFluxDensity.getBy();
final var bMeasZ = measuredMagneticFluxDensity.getBz();
final var bTrueX = expectedMagneticFluxDensity.getBx();
final var bTrueY = expectedMagneticFluxDensity.getBy();
final var bTrueZ = expectedMagneticFluxDensity.getBz();
a.setElementAt(i, 0, 1.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1194 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2138 |
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
// estimate Earth magnetic flux density at frame position and
// timestamp using WMM
final var ecefFrame = measurement.getFrame();
ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);
final var year = measurement.getYear();
final var latitude = nedFrame.getLatitude();
final var longitude = nedFrame.getLongitude();
final var height = nedFrame.getHeight();
nedFrame.getCoordinateTransformation(cbn);
cbn.inverse(cnb);
wmmEstimator.estimate(latitude, longitude, height, year, earthB);
// estimate expected body magnetic flux density taking into
// account body attitude (inverse of frame orientation) and
// estimated Earth magnetic flux density
BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);
final var bMeasX = measuredMagneticFluxDensity.getBx();
final var bMeasY = measuredMagneticFluxDensity.getBy();
final var bMeasZ = measuredMagneticFluxDensity.getBz();
final var bTrueX = expectedMagneticFluxDensity.getBx();
final var bTrueY = expectedMagneticFluxDensity.getBy();
final var bTrueZ = expectedMagneticFluxDensity.getBz();
a.setElementAt(i, 0, 1.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1509 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1517 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 704 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 814 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 185 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 823 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 597 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 581 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 190 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 187 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 902 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 909 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 900 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 905 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1955 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1943 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1948 |
super(position, measurements, commonAxisUsed, hardIron, initialMm, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates magnetometer calibration parameters containing soft-iron
* scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 1130 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 600 |
final Time timeInterval, final ECEFFrame frame, final ECEFFrame oldFrame) {
return estimateKinematicsAndReturnNew(timeInterval, frame, oldFrame);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final double x, final double y, final double z) {
return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final Speed vx, final Speed vy, final Speed vz, final Speed oldVx, final Speed oldVy, final Speed oldVz,
final double x, final double y, final double z) {
return estimateKinematicsAndReturnNew(timeInterval, c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param position cartesian body position resolved along ECEF-frame axes.
* @return a new body kinematics instance.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public BodyKinematics estimateAndReturnNew(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1676 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1695 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3399 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3464 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1955 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1944 |
super(measurements, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<PreliminaryResult>( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 5636 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 6156 |
estimatedMa = preliminaryResult.estimatedMa;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
}
/**
* Computes gravity norm for current position.
*
* @return gravity norm for current position expressed in meters per squared second.
*/
protected double computeGravityNorm() {
final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
position.getX(), position.getY(), position.getZ());
return gravity.getNorm();
}
/**
* Converts provided NED position expressed in terms of latitude, longitude and height respect
* mean Earth surface, to position expressed in ECEF coordinates.
*
* @param position NED position to be converted.
* @return converted position expressed in ECEF coordinates.
*/
private static ECEFPosition convertPosition(final NEDPosition position) {
final var velocity = new ECEFVelocity();
final var result = new ECEFPosition();
NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
position.getLatitude(), position.getLongitude(), position.getHeight(),
0.0, 0.0, 0.0, result, velocity);
return result;
}
/**
* Converts acceleration value and unit to meters per squared second.
*
* @param value acceleration value.
* @param unit unit of acceleration value.
* @return converted value.
*/
private static double convertAcceleration(final double value, final AccelerationUnit unit) {
return AccelerationConverter.convert(value, unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Converts acceleration instance to meters per squared second.
*
* @param acceleration acceleration instance to be converted.
* @return converted value.
*/
private static double convertAcceleration(final Acceleration acceleration) {
return convertAcceleration(acceleration.getValue().doubleValue(), acceleration.getUnit());
}
/**
* Internal class containing estimated preliminary result.
*/
protected static class PreliminaryResult {
/**
* Estimated accelerometer scale factors and cross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*/
private Matrix estimatedMa; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3796 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4436 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5115 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5799 |
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES);
jacobian.setElementAt(0, 0, m11);
jacobian.setElementAt(0, 1, m12);
jacobian.setElementAt(0, 2, m13);
jacobian.setElementAt(0, 3, bx);
jacobian.setElementAt(0, 4, by);
jacobian.setElementAt(0, 6, bz);
jacobian.setElementAt(1, 1, m22);
jacobian.setElementAt(1, 2, m23);
jacobian.setElementAt(1, 5, by);
jacobian.setElementAt(1, 7, bz);
jacobian.setElementAt(2, 2, m33);
jacobian.setElementAt(2, 8, bz);
jacobian.setElementAt(3, 3, 1.0);
jacobian.setElementAt(4, 5, 1.0);
jacobian.setElementAt(5, 8, 1.0);
jacobian.setElementAt(6, 4, 1.0);
jacobian.setElementAt(7, 6, 1.0);
jacobian.setElementAt(9, 7, 1.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java | 727 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 712 |
listener.onCalibrateProgressChange(LMedSRobustEasyGyroscopeCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
setupAccelerationFixer();
inliersData = null;
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.LMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5885 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 5403 |
innerCalibrator.getEstimatedBiases(result.estimatedBiases);
result.estimatedMg = innerCalibrator.getEstimatedMg();
result.estimatedGg = innerCalibrator.getEstimatedGg();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = sequences.size();
final var inlierSequences = new ArrayList<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierSequences.add(sequences.get(i));
}
}
try {
innerCalibrator.setGDependentCrossBiasesEstimated(estimateGDependentCrossBiases);
innerCalibrator.setInitialBias(preliminaryResult.estimatedBiases); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7470 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8599 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8763 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7949 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9059 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9220 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 318 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 317 |
LMedSRobustKnownFrameMagnetometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
setupWmmEstimator();
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.LMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1035 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1035 |
LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this,
progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
initialize();
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.LMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 814 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 759 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 823 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 768 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4909 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6349 |
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m21 = params[4];
final var m31 = params[5];
final var m12 = params[6];
final var m22 = params[7];
final var m32 = params[8];
final var m13 = params[9];
final var m23 = params[10];
final var m33 = params[11];
final var g11 = params[12];
final var g21 = params[13];
final var g31 = params[14];
final var g12 = params[15];
final var g22 = params[16];
final var g32 = params[17];
final var g13 = params[18];
final var g23 = params[19];
final var g33 = params[20];
return evaluate(i, bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3086 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4381 |
b = invInitM.multiplyAndReturnNew(bg);
fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
@Override
public int getNumberOfDimensions() {
// Before and after normalized gravity versors
return 2 * BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES];
// upper diagonal cross coupling errors M
var k = 0;
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
// g-dependent cross biases G
final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
for (int i = 0, j = k; i < num; i++, j++) {
initial[j] = initG.getElementAtIndex(i);
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1221 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1225 |
this(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm);
this.listener = listener;
}
/**
* Gets ground truth magnetic flux density norm to be expected at location where measurements have been made,
* expressed in Teslas (T).
*
* @return ground truth magnetic flux density or null.
*/
public Double getGroundTruthMagneticFluxDensityNorm() {
return groundTruthMagneticFluxDensityNorm;
}
/**
* Gets ground truth magnetic flux density norm to be expected at location where measurements have been made.
*
* @return ground truth magnetic flux density or null.
*/
public MagneticFluxDensity getGroundTruthMagneticFluxDensityNormAsMagneticFluxDensity() {
return groundTruthMagneticFluxDensityNorm != null
? new MagneticFluxDensity(groundTruthMagneticFluxDensityNorm, MagneticFluxDensityUnit.TESLA) : null;
}
/**
* Gets ground truth magnetic flux density norm to be expected at location where measurements have been made.
*
* @param result instance where result will be stored.
* @return true if ground truth magnetic flux density norm has been defined, false if it is not available yet.
*/
public boolean getGroundTruthMagneticFluxDensityNormAsMagneticFluxDensity(final MagneticFluxDensity result) {
if (groundTruthMagneticFluxDensityNorm != null) {
result.setValue(groundTruthMagneticFluxDensityNorm);
result.setUnit(MagneticFluxDensityUnit.TESLA);
return true;
} else {
return false;
}
}
/**
* Sets ground truth magnetic flux density norm to be expected at location where
* measurements have been made, expressed in Teslas (T).
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if calibrator is currently running.
*/
public void setGroundTruthMagneticFluxDensityNorm(final Double groundTruthMagneticFluxDensityNorm)
throws LockedException {
if (isRunning()) {
throw new LockedException();
}
internalSetGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm);
}
/**
* Sets ground truth magnetic flux density norm to be expected at location where
* measurements have been made.
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if calibrator is currently running.
*/
public void setGroundTruthMagneticFluxDensityNorm(final MagneticFluxDensity groundTruthMagneticFluxDensityNorm)
throws LockedException {
if (isRunning()) {
throw new LockedException();
}
if (groundTruthMagneticFluxDensityNorm != null) {
internalSetGroundTruthMagneticFluxDensityNorm(MagneticFluxDensityConverter.convert(
groundTruthMagneticFluxDensityNorm.getValue().doubleValue(),
groundTruthMagneticFluxDensityNorm.getUnit(),
MagneticFluxDensityUnit.TESLA));
} else {
internalSetGroundTruthMagneticFluxDensityNorm(null);
}
}
/**
* Gets known x-coordinate of magnetometer hard-iron bias.
* This is expressed in Teslas (T).
*
* @return known x-coordinate of magnetometer hard-iron bias.
*/
@Override
public double getHardIronX() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedMeasurementNoiseEstimator.java | 107 |
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java | 142 |
protected AccumulatedMeasurementNoiseEstimator(final L listener) {
this.listener = listener;
}
/**
* Gets time interval between accelerometer triad samples expressed in
* seconds (s).
*
* @return time interval between accelerometer triad samples.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between accelerometer triad samples expressed in
* seconds (s).
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval < 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between accelerometer triad samples.
*
* @return time interval between accelerometer triad samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between accelerometer triad samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between accelerometer triad samples.
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(),
timeInterval.getUnit(), TimeUnit.SECOND));
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public L getListener() {
return listener;
}
/**
* Sets listener to handle events raised by this estimator.
*
* @param listener listener to handle events raised by this estimator.
* @throws LockedException if this estimator is running.
*/
public void setListener(final L listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets last provided measurement or null if not available.
*
* @return last provided measurement or null.
*/
public M getLastMeasurement() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4683 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5719 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5220 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6237 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 552 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 552 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2157 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2156 |
setupWmmEstimator();
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 1624 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 967 |
final var specificForceZ = invAveCbe.getElementAtIndex(2);
// save result data
result.setSpecificForceCoordinates(specificForceX, specificForceY, specificForceZ);
result.setAngularRateCoordinates(angularRateX, angularRateY, angularRateZ);
} catch (final AlgebraException ignore) {
// never happens
}
} else {
// If time interval is zero, set angular rate and specific force to zero
result.setSpecificForceCoordinates(0.0, 0.0, 0.0);
result.setAngularRateCoordinates(0.0, 0.0, 0.0);
}
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final Time timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC,
final double vx, final double vy, final double vz,
final double oldVx, final double oldVy, final double oldVz,
final double x, final double y, final double z, final BodyKinematics result) {
estimateKinematics(
TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
c, oldC, vx, vy, vz, oldVx, oldVy, oldVz, x, y, z, result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c boy-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param velocity velocity of body frame with respect ECEF frame.
* @param oldVelocity previous velocity of body frame with respect ECEF frame.
* @param x cartesian x coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param y cartesian y coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param z cartesian z coordinate of body position expressed in meters (m)
* with respect ECEF frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3162 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4461 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5007 |
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mm.setElementAtIndex(0, m11);
mm.setElementAtIndex(1, 0.0);
mm.setElementAtIndex(2, 0.0);
mm.setElementAtIndex(3, m12);
mm.setElementAtIndex(4, m22);
mm.setElementAtIndex(5, 0.0);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33);
final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mg.setElementAtIndex(0, g11);
mg.setElementAtIndex(1, g21);
mg.setElementAtIndex(2, g31);
mg.setElementAtIndex(3, g12);
mg.setElementAtIndex(4, g22);
mg.setElementAtIndex(5, g32);
mg.setElementAtIndex(6, g13);
mg.setElementAtIndex(7, g23);
mg.setElementAtIndex(8, g33);
setResult(mm, mg); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3433 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4760 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5352 |
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mm.setElementAtIndex(0, m11);
mm.setElementAtIndex(1, m21);
mm.setElementAtIndex(2, m31);
mm.setElementAtIndex(3, m12);
mm.setElementAtIndex(4, m22);
mm.setElementAtIndex(5, m32);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33);
final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mg.setElementAtIndex(0, g11);
mg.setElementAtIndex(1, g21);
mg.setElementAtIndex(2, g31);
mg.setElementAtIndex(3, g12);
mg.setElementAtIndex(4, g22);
mg.setElementAtIndex(5, g32);
mg.setElementAtIndex(6, g13);
mg.setElementAtIndex(7, g23);
mg.setElementAtIndex(8, g33);
setResult(mm, mg); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5295 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5448 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6332 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6485 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5816 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5966 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6833 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6983 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4397 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4934 |
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
// g-dependent cross biases G
final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
for (int i = 0, j = k; i < num; i++, j++) {
initial[j] = initG.getElementAtIndex(i);
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException {
measAngularRateX = point[0];
measAngularRateY = point[1];
measAngularRateZ = point[2];
fmeasX = point[3];
fmeasY = point[4];
fmeasZ = point[5];
gradientEstimator.gradient(params, derivatives);
return evaluateCommonAxisWitGDependentCrossBiases(params); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1414 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1385 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 750 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 735 |
PROSACRobustEasyGyroscopeCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
setupAccelerationFixer();
inliersData = null;
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4748 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4873 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5072 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6109 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5784 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5910 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5282 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5401 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5594 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6611 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6299 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6418 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 538 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 538 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 342 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 341 |
PROSACRobustKnownFrameMagnetometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
setupWmmEstimator();
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2143 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2142 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1061 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1060 |
PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
initialize();
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 832 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 945 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 892 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 316 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 953 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java | 899 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 839 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 1772 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 321 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java | 1805 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1034 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1031 |
LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.LMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1509 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1576 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1594 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1517 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 344 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 344 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 346 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1944 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java | 678 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java | 665 |
listener.onCalibrateProgressChange(MSACRobustEasyGyroscopeCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
setupAccelerationFixer();
inliersData = null;
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.MSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4524 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4627 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4808 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5845 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4937 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5974 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events
* raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4939 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5074 |
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events
* raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5560 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5663 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(position,
turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5976 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6111 |
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events
* raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5058 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5163 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5339 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6356 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5461 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6478 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5463 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5596 |
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6075 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6180 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6480 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6613 |
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 271 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 270 |
MSACRobustKnownFrameMagnetometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
setupWmmEstimator();
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.MSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 987 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 988 |
MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
initialize();
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.MSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 13634 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 6391 |
private static double convertTimeToDouble(final Time time) {
return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
}
/**
* Converts provided distance instance into its corresponding value expressed in
* meters.
*
* @param distance distance instance to be converted.
* @return converted value expressed in meters.
*/
private static double convertDistanceToDouble(final Distance distance) {
return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
}
/**
* Converts provided speed instance into its corresponding value expressed in
* meters per second.
*
* @param speed speed instance to be converted.
* @return converted value expressed in meters per second.
*/
private static double convertSpeedToDouble(final Speed speed) {
return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
}
/**
* Converts provided acceleration instance into its corresponding value expressed
* in meters per squared second.
*
* @param acceleration acceleration instance to be converted.
* @return converted value expressed in meters per squared second.
*/
private static double convertAccelerationToDouble(final Acceleration acceleration) {
return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Converts provided angular speed into its corresponding value expressed in
* radians per second.
*
* @param angularSpeed angular speed instance to be converted.
* @return converted value expressed in radians per second.
*/
private static double convertAngularSpeedToDouble(final AngularSpeed angularSpeed) {
return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit(),
AngularSpeedUnit.RADIANS_PER_SECOND);
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java | 1952 |
| com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java | 2197 |
public boolean equals(final INSLooselyCoupledKalmanState other, final double threshold) {
if (other == null) {
return false;
}
return Math.abs(vx - other.vx) <= threshold
&& Math.abs(vy - other.vy) <= threshold
&& Math.abs(vz - other.vz) <= threshold
&& Math.abs(x - other.x) <= threshold
&& Math.abs(y - other.y) <= threshold
&& Math.abs(z - other.z) <= threshold
&& Math.abs(accelerationBiasX - other.accelerationBiasX) <= threshold
&& Math.abs(accelerationBiasY - other.accelerationBiasY) <= threshold
&& Math.abs(accelerationBiasZ - other.accelerationBiasZ) <= threshold
&& Math.abs(gyroBiasX - other.gyroBiasX) <= threshold
&& Math.abs(gyroBiasY - other.gyroBiasY) <= threshold
&& Math.abs(gyroBiasZ - other.gyroBiasZ) <= threshold
&& other.bodyToEcefCoordinateTransformationMatrix != null | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 814 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 768 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 759 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 823 |
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4883 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4982 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4404 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4502 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7389 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8683 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must
* have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7554 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8517 |
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must be
* 3x1 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7866 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9139 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8029 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8976 |
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias,
initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 405 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1285 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1580 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1255 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 409 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 408 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2010 |
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5787 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6563 |
ftrue = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (tmp == null) {
tmp = new Matrix(BodyKinematics.COMPONENTS, 1);
}
measAngularRate.setElementAtIndex(0, measAngularRateX);
measAngularRate.setElementAtIndex(1, measAngularRateY);
measAngularRate.setElementAtIndex(2, measAngularRateZ);
fmeas.setElementAtIndex(0, fmeasX);
fmeas.setElementAtIndex(1, fmeasY);
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, biasX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6747 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7875 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6813 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6949 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7941 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8077 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7238 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8346 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7304 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7434 |
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8412 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8542 |
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1704 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1869 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1773 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 535 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1889 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1795 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 857 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 969 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 916 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 978 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 925 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1710 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3595 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 539 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3658 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 863 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1796 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 345 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1828 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2141 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2138 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1058 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1055 |
PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 704 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 759 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 185 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 768 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 597 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 581 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 190 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 187 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 902 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 909 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 900 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 905 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7161 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8436 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7308 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8289 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7640 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8894 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7784 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8748 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/noise/WindowedMeasurementNoiseEstimator.java | 182 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java | 202 |
reset();
}
/**
* Gets time interval between accelerometer triad samples expressed in
* seconds (s).
*
* @return time interval between accelerometer triad samples.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between accelerometer triad samples expressed in
* seconds (s).
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval < 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between accelerometer triad samples.
*
* @return time interval between accelerometer triad samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between accelerometer triad samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between accelerometer triad samples.
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
TimeUnit.SECOND));
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public L getListener() {
return listener;
}
/**
* Sets listener to handle events raised by this estimator.
*
* @param listener listener to handle events raised by this estimator.
* @throws LockedException if this estimator is running.
*/
public void setListener(final L listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets first provided measurement value expressed in its default units.
* (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
*
* @return first provided measurement value or null if not available.
*/
public Double getFirstWindowedMeasurementValue() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/AccelerationFixer.java | 205 |
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 231 |
| com/irurueta/navigation/inertial/calibration/MagneticFluxDensityFixer.java | 234 |
final var biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
this.bias.setElementAtIndex(0, biasX);
this.bias.setElementAtIndex(1, biasY);
this.bias.setElementAtIndex(2, biasZ);
}
/**
* Gets x-coordinate of bias expressed in meters per squared second
* (m/s^2).
*
* @return x-coordinate of bias expressed in meters per squared
* second (m/s^2).
*/
public double getBiasX() {
return bias.getElementAtIndex(0);
}
/**
* Sets x-coordinate of bias expressed in meters per squared second
* (m/s^2).
*
* @param biasX x-coordinate of bias expressed in meters per squared
* second (m/s^2).
*/
public void setBiasX(final double biasX) {
bias.setElementAtIndex(0, biasX);
}
/**
* Gets y-coordinate of bias expressed in meters per squared second
* (m/s^2).
*
* @return y-coordinate of bias expressed in meters per squared
* second (m/s^2).
*/
public double getBiasY() {
return bias.getElementAtIndex(1);
}
/**
* Sets y-coordinate of bias expressed in meters per squared second
* (m/s^2).
*
* @param biasY y-coordinate of bias expressed in meters per squared
* second (m/s^2).
*/
public void setBiasY(final double biasY) {
bias.setElementAtIndex(1, biasY);
}
/**
* Gets z-coordinate of bias expressed in meters per squared second
* (m/s^2).
*
* @return z-coordinate of bias expressed in meters per squared
* second (m/s^2).
*/
public double getBiasZ() {
return bias.getElementAtIndex(2);
}
/**
* Sets z-coordinate of bias expressed in meters per squared second
* (m/s^2).
*
* @param biasZ z-coordinate of bias expressed in meters per squared
* second (m/s^2).
*/
public void setBiasZ(final double biasZ) {
bias.setElementAtIndex(2, biasZ);
}
/**
* Sets coordinates of bias expressed in meters per squared second
* (m/s^2).
*
* @param biasX x-coordinate of bias.
* @param biasY y-coordinate of bias.
* @param biasZ z-coordinate of bias.
*/
public void setBias(final double biasX, final double biasY, final double biasZ) {
setBiasX(biasX);
setBiasY(biasY);
setBiasZ(biasZ);
}
/**
* Gets x-coordinate of bias.
*
* @return x-coordinate of bias.
*/
public Acceleration getBiasXAsAcceleration() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5817 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6593 |
b.setElementAtIndex(2, biasZ);
g.setElementAt(0, 0, g11);
g.setElementAt(1, 0, g21);
g.setElementAt(2, 0, g31);
g.setElementAt(0, 1, g12);
g.setElementAt(1, 1, g22);
g.setElementAt(2, 1, g32);
g.setElementAt(0, 2, g13);
g.setElementAt(1, 2, g23);
g.setElementAt(2, 2, g33);
getAccelerometerBiasAsMatrix(ba);
getAccelerometerMa(ma);
// fix measured accelerometer value to obtain true
// specific force
accelerationFixer.fix(fmeas, ftrue);
g.multiply(ftrue, tmp);
invM.multiply(measAngularRate, trueAngularRate);
trueAngularRate.subtract(b);
trueAngularRate.subtract(tmp);
final var norm = Utils.normF(trueAngularRate);
return norm * norm;
} catch (final AlgebraException e) {
throw new EvaluationException(e);
}
}
/**
* Computes estimated true angular rate squared norm using current measured
* angular rate and provided parameters.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fittin needed for calibration computation.
*
* @param m11 element 1,1 of cross-coupling error matrix.
* @param m21 element 2,1 of cross-coupling error matrix.
* @param m31 element 3,1 of cross-coupling error matrix.
* @param m12 element 1,2 of cross-coupling error matrix.
* @param m22 element 2,2 of cross-coupling error matrix.
* @param m32 element 3,2 of cross-coupling error matrix.
* @param m13 element 1,3 of cross-coupling error matrix.
* @param m23 element 2,3 of cross-coupling error matrix.
* @param m33 element 3,3 of cross-coupling error matrix.
* @return estimated true angular rate squared norm.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluate(final double m11, final double m21, final double m31, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6630 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7758 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7122 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8230 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 783 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 898 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 846 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 268 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 905 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java | 852 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 794 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1725 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 274 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1757 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 990 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 983 |
MSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
}
}
});
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
inliersData = null;
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.MSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1615 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 452 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2060 |
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Matrix>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1782 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1801 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1623 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3505 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 450 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3570 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 450 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2051 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2048 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2053 |
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1636 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 471 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2076 |
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1799 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 465 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1819 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1640 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3525 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 469 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3588 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 468 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2070 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2068 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2072 |
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1115 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java | 1117 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 6555 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 7061 |
}
/**
* Converts a time instance expressed in milliseconds since epoch time
* (January 1st, 1970 at midnight) to a decimal year.
*
* @param timestampMillis milliseconds value to be converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final Long timestampMillis) {
if (timestampMillis == null) {
return null;
}
final var calendar = new GregorianCalendar();
calendar.setTimeInMillis(timestampMillis);
return convertTime(calendar);
}
/**
* Converts a time instant contained ina date object to a
* decimal year.
*
* @param date a time instance to be converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final Date date) {
if (date == null) {
return null;
}
final var calendar = new GregorianCalendar();
calendar.setTime(date);
return convertTime(calendar);
}
/**
* Converts a time instant contained in a gregorian calendar to a
* decimal year.
*
* @param calendar calendar containing a specific instant to be
* converted.
* @return converted value expressed in decimal years.
*/
private static Double convertTime(final GregorianCalendar calendar) {
if (calendar == null) {
return null;
}
return WMMEarthMagneticFluxDensityEstimator.convertTime(calendar);
}
/**
* Converts provided ECEF position to position expressed in NED
* coordinates.
*
* @param position ECEF position to be converted.
* @return converted position expressed in NED coordinates.
*/
private static NEDPosition convertPosition(final ECEFPosition position) {
final var velocity = new NEDVelocity();
final var result = new NEDPosition();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
position.getX(), position.getY(), position.getZ(),
0.0, 0.0, 0.0, result, velocity);
return result;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java | 144 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java | 203 |
}
/**
* Gets time interval between triad samples expressed in
* seconds (s).
*
* @return time interval between accelerometer triad samples.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between triad samples expressed in
* seconds (s).
*
* @param timeInterval time interval between triad samples.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval < 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between triad samples.
*
* @return time interval between triad samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between triad samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between triad samples.
*
* @param timeInterval time interval between triad samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
TimeUnit.SECOND));
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public L getListener() {
return listener;
}
/**
* Sets listener to handle events raised by this estimator.
*
* @param listener listener to handle events raised by this estimator.
* @throws LockedException if this estimator is running.
*/
public void setListener(final L listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets last provided triad values or null if not available.
*
* @return last provided triad values or null.
*/
public T getLastTriad() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 535 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 533 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2144 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2140 |
setupWmmEstimator();
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return true;
}
/**
* Sets quality scores corresponding to each provided sample.
* This method is used internally and does not check whether instance is
* locked or not.
*
* @param qualityScores quality scores to be set.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
private void internalSetQualityScores(final double[] qualityScores) {
if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedMeasurementNoiseEstimator.java | 109 |
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java | 144 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedMeasurementNoiseEstimator.java | 183 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java | 203 |
}
/**
* Gets time interval between accelerometer triad samples expressed in
* seconds (s).
*
* @return time interval between accelerometer triad samples.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between accelerometer triad samples expressed in
* seconds (s).
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval < 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between accelerometer triad samples.
*
* @return time interval between accelerometer triad samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between accelerometer triad samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between accelerometer triad samples.
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(),
timeInterval.getUnit(), TimeUnit.SECOND));
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public L getListener() {
return listener;
}
/**
* Sets listener to handle events raised by this estimator.
*
* @param listener listener to handle events raised by this estimator.
* @throws LockedException if this estimator is running.
*/
public void setListener(final L listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets last provided measurement or null if not available.
*
* @return last provided measurement or null.
*/
public M getLastMeasurement() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6881 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8145 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7017 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8009 |
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7369 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8607 |
final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7499 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8477 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3025 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3158 |
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurementsOrSequences() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the calibration.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanEpochEstimator.java | 704 |
| com/irurueta/navigation/inertial/INSTightlyCoupledKalmanEpochEstimator.java | 185 |
INSLooselyCoupledKalmanState.NUM_PARAMS);
final var gyroNoisePSD = config.getGyroNoisePSD();
final var gyroNoiseValue = gyroNoisePSD * propagationInterval;
for (var i = 0; i < 3; i++) {
qPrimeMatrix.setElementAt(i, i, gyroNoiseValue);
}
final var accelNoisePSD = config.getAccelerometerNoisePSD();
final var accelNoiseValue = accelNoisePSD * propagationInterval;
for (var i = 3; i < 6; i++) {
qPrimeMatrix.setElementAt(i, i, accelNoiseValue);
}
final var accelBiasPSD = config.getAccelerometerBiasPSD();
final var accelBiasValue = accelBiasPSD * propagationInterval;
for (var i = 9; i < 12; i++) {
qPrimeMatrix.setElementAt(i, i, accelBiasValue);
}
final var gyroBiasPSD = config.getGyroBiasPSD();
final var gyroBiasValue = gyroBiasPSD * propagationInterval;
for (var i = 12; i < 15; i++) {
qPrimeMatrix.setElementAt(i, i, gyroBiasValue);
}
// 3. Propagate state estimates using (3.14) noting that all states are zero
// due to closed-loop correction.
// x_est_propagated(1:15, 1) = 0
// 4. Propagate state estimation error covariance matrix using (3.46)
final var pMatrixOld = previousState.getCovariance(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1060 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1196 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4925 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2140 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2278 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4317 |
var i = 0;
for (final var measurement : measurements) {
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
// estimate Earth magnetic flux density at frame position and
// timestamp using WMM
final var ecefFrame = measurement.getFrame();
ECEFtoNEDFrameConverter.convertECEFtoNED(ecefFrame, nedFrame);
final var year = measurement.getYear();
final var latitude = nedFrame.getLatitude();
final var longitude = nedFrame.getLongitude();
final var height = nedFrame.getHeight();
nedFrame.getCoordinateTransformation(cbn);
cbn.inverse(cnb);
wmmEstimator.estimate(latitude, longitude, height, year, earthB);
// estimate expected body magnetic flux density taking into
// account body attitude (inverse of frame orientation) and
// estimated Earth magnetic flux density
BodyMagneticFluxDensityEstimator.estimate(earthB, cnb, expectedMagneticFluxDensity);
final var bMeasX = measuredMagneticFluxDensity.getBx();
final var bMeasY = measuredMagneticFluxDensity.getBy();
final var bMeasZ = measuredMagneticFluxDensity.getBz();
final var bTrueX = expectedMagneticFluxDensity.getBx();
final var bTrueY = expectedMagneticFluxDensity.getBy();
final var bTrueZ = expectedMagneticFluxDensity.getBz(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedMeasurementNoiseEstimator.java | 212 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedMeasurementNoiseEstimator.java | 343 |
result.setUnit(lastMeasurement.getUnit());
return true;
} else {
return false;
}
}
/**
* Gets estimated average of measurement expressed in its default unit
* (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
*
* @return average of measurement in current window.
*/
public double getAvg() {
return avg;
}
/**
* Gets estimated average of measurement within current window.
*
* @return average of measurement in current window
*/
public M getAvgAsMeasurement() {
return createMeasurement(avg, getDefaultUnit());
}
/**
* Gets estimated average of measurement within current window.
*
* @param result instance where average of measurement will be stored.
*/
public void getAvgAsMeasurement(final M result) {
result.setValue(avg);
result.setUnit(getDefaultUnit());
}
/**
* Gets estimated variance of measurement within current window
* expressed in its default squared unit (m^2/s^4 for acceleration,
* rad^2/s^2 for angular speed or T^2 for magnetic flux density).
*
* @return estimated variance of measurement within current window.
*/
public double getVariance() {
return variance;
}
/**
* Gets estimated standard deviation of measurement within current window
* and expressed in its default unit (m/s^2 for acceleration, rad/s for
* angular speed or T for magnetic flux density).
*
* @return estimated standard of measurement.
*/
public double getStandardDeviation() {
return Math.sqrt(variance);
}
/**
* Gets estimated standard deviation of measurement within current window.
*
* @return estimated standard deviation of measurement.
*/
public M getStandardDeviationAsMeasurement() {
return createMeasurement(getStandardDeviation(), getDefaultUnit());
}
/**
* Gets estimated standard deviation of measurement within current window.
*
* @param result instance where estimated standard deviation of measurement
* will be stored.
*/
public void getStandardDeviationAsMeasurement(final M result) {
result.setValue(getStandardDeviation());
result.setUnit(getDefaultUnit());
}
/**
* Gets measurement noise PSD (Power Spectral Density) expressed
* in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
* magnetometer.
*
* @return measurement noise PSD.
*/
public double getPsd() {
return variance * timeInterval;
}
/**
* Gets measurement noise root PSD (Power Spectral Density) expressed in
* (m * s^-1.5) for accelerometer, (rad * s^-0.5) for gyroscope or (T * s^0.5) for
* magnetometer.
*
* @return measurement noise root PSD.
*/
public double getRootPsd() {
return Math.sqrt(getPsd());
}
/**
* Gets number of samples that have been processed so far.
*
* @return number of samples that have been processed so far.
*/
public int getNumberOfProcessedSamples() {
return numberOfProcessedSamples;
}
/**
* Indicates whether estimator is currently running or not.
*
* @return true if estimator is running, false otherwise.
*/
public boolean isRunning() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6039 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5633 |
final var initialB = invInitialM.multiplyAndReturnNew(initialBa);
fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
@Override
public int getNumberOfDimensions() {
// Input points are measured specific force coordinates
return BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[COMMON_Z_AXIS_UNKNOWNS];
// biases b
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
initial[i] = initialB.getElementAtIndex(i);
}
// upper diagonal cross coupling errors M
var k = BodyKinematics.COMPONENTS;
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 3025 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 3112 |
result.estimatedMa = nonLinearCalibrator.getEstimatedMa();
if (keepCovariance) {
result.covariance = nonLinearCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = nonLinearCalibrator.getEstimatedMse();
result.estimatedChiSq = nonLinearCalibrator.getEstimatedChiSq();
}
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationFrameBodyKinematics>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierMeasurements.add(measurements.get(i));
}
}
try {
nonLinearCalibrator.setInitialBias(preliminaryResult.estimatedBiases);
nonLinearCalibrator.setInitialMa(preliminaryResult.estimatedMa); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4087 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4174 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3597 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3684 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5219 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6410 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must
* have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5373 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6256 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This must be
* 3x1 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5738 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6907 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5890 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6755 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer. Must
* be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 5819 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 5569 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 6364 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 6087 |
innerCalibrator.calibrate();
result.estimatedMa = innerCalibrator.getEstimatedMa();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierMeasurements.add(measurements.get(i));
}
}
try {
innerCalibrator.setBiasCoordinates(biasX, biasY, biasZ); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4626 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5662 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4685 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4810 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5721 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5847 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5162 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6179 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5222 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5341 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6239 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6358 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 7493 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 8020 |
innerCalibrator.calibrate();
result.estimatedMm = innerCalibrator.getEstimatedMm();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationBodyMagneticFluxDensity>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierMeasurements.add(measurements.get(i));
}
}
try {
innerCalibrator.setHardIronCoordinates(hardIronX, hardIronY, hardIronZ); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java | 1110 |
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java | 1012 |
public void setListener(final INSGNSSTightlyCoupledKalmanFilteredEstimatorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum epoch interval expressed in seconds (s) between consecutive
* propagations or measurements expressed in seconds.
* Attempting to propagate results using Kalman filter or updating measurements
* when intervals are less than this value, will be ignored.
*
* @return minimum epoch interval between consecutive propagations or
* measurements.
*/
public double getEpochInterval() {
return epochInterval;
}
/**
* Sets minimum epoch interval expressed in seconds (s) between consecutive
* propagations or measurements expressed in seconds.
* Attempting to propagate results using Kalman filter or updating measurements
* when intervals are less than this value, will be ignored.
*
* @param epochInterval minimum epoch interval expressed in seconds (s) between
* consecutive propagations or measurements.
* @throws LockedException if this estimator is already running.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public void setEpochInterval(final double epochInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (epochInterval < 0.0) {
throw new IllegalArgumentException();
}
this.epochInterval = epochInterval;
}
/**
* Gets minimum epoch interval between consecutive propagations or measurements.
* Attempting to propagate results using Kalman filter or updating measurements
* when intervals are less than this value, will be ignored.
*
* @param result instance where minimum epoch interval will be stored.
*/
public void getEpochIntervalAsTime(final Time result) {
result.setValue(epochInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Gets minimum epoch interval between consecutive propagations or measurements.
* Attempting to propagate results using Kalman filter or updating measurements
* when intervals are less than this value, will be ignored.
*
* @return minimum epoch interval.
*/
public Time getEpochIntervalAsTime() {
return new Time(epochInterval, TimeUnit.SECOND);
}
/**
* Sets minimum epoch interval between consecutive propagations or measurements.
* Attempting to propagate results using Kalman filter or updating measurements
* when intervals are less than this value, will be ignored.
*
* @param epochInterval minimum epoch interval.
* @throws LockedException if this estimator is already running.
* @throws IllegalArgumentException if provided epoch interval is negative.
*/
public void setEpochInterval(final Time epochInterval) throws LockedException {
final var epochIntervalSeconds = TimeConverter.convert(epochInterval.getValue().doubleValue(),
epochInterval.getUnit(), TimeUnit.SECOND);
setEpochInterval(epochIntervalSeconds);
}
/**
* Gets INS/GNSS tightly coupled Kalman configuration parameters (usually
* obtained through calibration).
*
* @param result instance where INS/GNSS tightly coupled Kalman configuration
* parameters will be stored.
* @return true if result instance is updated, false otherwise.
*/
public boolean getConfig(final INSTightlyCoupledKalmanConfig result) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3652 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4971 |
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var m11 = result[3];
final var m12 = result[4];
final var m22 = result[5];
final var m13 = result[6];
final var m23 = result[7];
final var m33 = result[8];
final var g11 = result[9];
final var g21 = result[10];
final var g31 = result[11];
final var g12 = result[12];
final var g22 = result[13];
final var g32 = result[14];
final var g13 = result[15];
final var g23 = result[16];
final var g33 = result[17];
final var b = new Matrix(BodyKinematics.COMPONENTS, 1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1369 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 1048 |
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var omegaMeasX = measuredKinematics.getAngularRateX();
final var omegaMeasY = measuredKinematics.getAngularRateY();
final var omegaMeasZ = measuredKinematics.getAngularRateZ();
final var omegaTrueX = expectedKinematics.getAngularRateX();
final var omegaTrueY = expectedKinematics.getAngularRateY();
final var omegaTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, omegaTrueX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1500 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 907 |
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var omegaMeasX = measuredKinematics.getAngularRateX();
final var omegaMeasY = measuredKinematics.getAngularRateY();
final var omegaMeasZ = measuredKinematics.getAngularRateZ();
final var omegaTrueX = expectedKinematics.getAngularRateX();
final var omegaTrueY = expectedKinematics.getAngularRateY();
final var omegaTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, omegaTrueX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5039 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5164 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4559 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4684 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5006 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6180 |
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5143 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6043 |
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5292 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7467 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6329 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8596 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5527 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6678 |
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5661 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6544 |
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated,
* false otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5813 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7946 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6830 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9056 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2696 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3033 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 5749 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 5498 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 6287 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 6010 |
final var estMa = preliminaryResult.estimatedMa;
if (identity == null) {
identity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp1 == null) {
tmp1 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp2 == null) {
tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp3 == null) {
tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (tmp4 == null) {
tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
identity.add(estMa, tmp1);
Utils.inverse(tmp1, tmp2);
final var kinematics = measurement.getKinematics();
final var fmeasX = kinematics.getFx();
final var fmeasY = kinematics.getFy();
final var fmeasZ = kinematics.getFz(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1819 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1866 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3835 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3795 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3847 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3882 |
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @return position where body kinematics measures have been taken.
*/
public ECEFPosition getEcefPosition() {
return position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* ECEF coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final ECEFPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = position;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @return position where body kinematics measures have been taken or null if
* not available.
*/
public NEDPosition getNedPosition() {
final var result = new NEDPosition();
return getNedPosition(result) ? result : null;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param result instance where result will be stored.
* @return true if NED position could be computed, false otherwise.
*/
public boolean getNedPosition(final NEDPosition result) {
if (position != null) {
final var velocity = new NEDVelocity();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(position.getX(), position.getY(), position.getZ(),
0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final NEDPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
this.position = convertPosition(position);
}
/**
* Gets a list of body kinematics measurements taken at a
* given position with different unknown orientations and containing the
* standard deviations of accelerometer and gyroscope measurements.
*
* @return list of body kinematics measurements.
*/
@Override
public List<StandardDeviationBodyKinematics> getMeasurements() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4523 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5559 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5057 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6074 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 7423 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 7943 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 6907 |
final var estMm = preliminaryResult.estimatedMm;
if (identity == null) {
identity = Matrix.identity(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
}
if (tmp1 == null) {
tmp1 = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
}
if (tmp2 == null) {
tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp3 == null) {
tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (tmp4 == null) {
tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
identity.add(estMm, tmp1);
Utils.inverse(tmp1, tmp2);
final var measuredMagneticFluxDensity = measurement.getMagneticFluxDensity();
final var bMeasX = measuredMagneticFluxDensity.getBx();
final var bMeasY = measuredMagneticFluxDensity.getBy();
final var bMeasZ = measuredMagneticFluxDensity.getBz(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2696 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3166 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2918 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3033 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5292 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8596 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6329 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7467 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5813 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9056 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6830 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7946 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6688 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7816 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final double[] bias, final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7179 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8287 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 356 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 355 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1075 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1074 |
setupWmmEstimator();
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.RANSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4748 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5910 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4873 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5784 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5445 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7631 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6482 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8760 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7089 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8362 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events
* raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7234 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8217 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5282 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6418 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5401 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6299 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5963 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8107 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6980 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9217 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7569 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8822 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7712 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8677 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2681 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 395 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2903 |
public void setListener(final KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 721 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 208 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 925 |
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 833 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 203 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 840 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 729 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1660 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 209 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1692 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 206 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 924 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 918 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 923 |
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 904 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 275 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 913 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 798 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1731 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 280 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1763 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 277 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 992 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 990 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 995 |
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 406 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3152 |
public void setListener(final KnownFrameGyroscopeLinearLeastSquaresCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurementsOrSequences() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the calibration.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6571 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7699 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6572 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6690 |
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7700 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7817 |
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7065 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8171 |
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7067 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7181 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8173 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8289 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final NEDPosition position, final double turntableRotationRate,
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1081 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 401 |
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether estimator is ready to start the estimator.
*
* @return true if estimator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether estimator is currently running or not.
*
* @return true if estimator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if estimator is currently running.
* @throws NotReadyException if estimator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1527 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 1062 |
}
/**
* Fills scale factor and cross coupling error matrix with estimated values.
*
* @param sx x scale factor
* @param sy y scale factor
* @param sz z scale factor
* @param mxy x-y cross coupling
* @param mxz x-z cross coupling
* @param myx y-x cross coupling
* @param myz y-z cross coupling
* @param mzx z-x cross coupling
* @param mzy z-y cross coupling
* @throws WrongSizeException never happens.
*/
private void fillMa(final double sx, final double sy, final double sz,
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy) throws WrongSizeException {
if (estimatedMa == null) {
estimatedMa = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
estimatedMa.setElementAt(0, 0, sx);
estimatedMa.setElementAt(1, 0, myx);
estimatedMa.setElementAt(2, 0, mzx);
estimatedMa.setElementAt(0, 1, mxy);
estimatedMa.setElementAt(1, 1, sy);
estimatedMa.setElementAt(2, 1, mzy);
estimatedMa.setElementAt(0, 2, mxz);
estimatedMa.setElementAt(1, 2, myz);
estimatedMa.setElementAt(2, 2, sz);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4957 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6396 |
private double evaluateCommonAxisWithGDependentCrossBiases(final int i, final double[] params)
throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m12 = params[4];
final var m22 = params[5];
final var m13 = params[6];
final var m23 = params[7];
final var m33 = params[8];
final var g11 = params[9];
final var g21 = params[10];
final var g31 = params[11];
final var g12 = params[12];
final var g22 = params[13];
final var g32 = params[14];
final var g13 = params[15];
final var g23 = params[16];
final var g33 = params[17];
return evaluate(i, bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1085 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 413 |
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurementsOrSequences() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the calibration.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated gyroscope scale factors and cross coupling errors.
* This is the product of matrix Tg containing cross coupling errors and Kg
* containing scaling factors.
* So that:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Kg = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tg = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mg = [sx mxy mxz] = Tg*Kg = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the gyroscope z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mg matrix
* becomes upper diagonal:
* <pre>
* Mg = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated gyroscope scale factors and cross coupling errors.
*/
@Override
public Matrix getEstimatedMg() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4194 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5600 |
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
throws EvaluationException {
final var m11 = params[0];
final var m21 = params[1];
final var m31 = params[2];
final var m12 = params[3];
final var m22 = params[4];
final var m32 = params[5];
final var m13 = params[6];
final var m23 = params[7];
final var m33 = params[8];
final var g11 = params[9];
final var g21 = params[10];
final var g31 = params[11];
final var g12 = params[12];
final var g22 = params[13];
final var g32 = params[14];
final var g13 = params[15];
final var g23 = params[16];
final var g33 = params[17];
return evaluate(i, m11, m21, m31, m12, m22, m32, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MidPointQuaternionStepIntegrator.java | 134 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RungeKuttaQuaternionStepIntegrator.java | 163 |
omega0, omega1, omega01, quat, quatResult, tmpQ, k1, k2, omegaSkew);
}
/**
* Performs a mid-point integration step.
* More information available here:
* <a href="https://en.wikipedia.org/wiki/Midpoint_method">https://en.wikipedia.org/wiki/Midpoint_method</a>
*
* @param initialAttitude initial attitude.
* @param initialWx initial x-coordinate rotation velocity at initial timestamp expressed
* in radians per second (rad/s).
* @param initialWy initial y-coordinate rotation velocity at initial timestamp expressed
* in radians per second (rad/s).
* @param initialWz initial z-coordinate rotation velocity at initial timestamp expressed
* in radians per second (rad/s).
* @param currentWx end x-coordinate rotation velocity at end timestamp expressed in
* radians per second (rad/s).
* @param currentWy end y-coordinate rotation velocity at end timestamp expressed in
* radians per second (rad/s).
* @param currentWz end z-coordinate rotation velocity at end timestamp expressed in
* radians per second (rad/s).
* @param dt time step expressed in seconds (t1 - t0).
* @param result instance where result of integration will be stored.
* @throws RotationException if a numerical error occurs.
*/
public static void integrationStep(
final Quaternion initialAttitude,
final double initialWx, final double initialWy, final double initialWz,
final double currentWx, final double currentWy, final double currentWz,
final double dt, final Quaternion result) throws RotationException {
try {
final var omega0 = new Matrix(Rotation3D.INHOM_COORDS, 1);
final var omega1 = new Matrix(Rotation3D.INHOM_COORDS, 1);
final var omega01 = new Matrix(Rotation3D.INHOM_COORDS, 1);
final var quat = new Matrix(Quaternion.N_PARAMS, 1);
final var quatResult = new Matrix(Quaternion.N_PARAMS, 1);
final var tmpQ = new Matrix(Quaternion.N_PARAMS, 1);
final var k1 = new Matrix(Quaternion.N_PARAMS, 1);
final var k2 = new Matrix(Quaternion.N_PARAMS, 1);
final var omegaSkew = new Matrix(Quaternion.N_PARAMS, Quaternion.N_PARAMS); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 5257 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10445 |
final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
m1.add(mg);
final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
angularRateTrue.setElementAtIndex(0, angularRateTrueX);
angularRateTrue.setElementAtIndex(1, angularRateTrueY);
angularRateTrue.setElementAtIndex(2, angularRateTrueZ);
m1.multiply(angularRateTrue);
final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
fTrue.setElementAtIndex(0, fTrueX);
fTrue.setElementAtIndex(1, fTrueY);
fTrue.setElementAtIndex(2, fTrueZ);
final var m2 = gg.multiplyAndReturnNew(fTrue);
m1.add(m2);
final var angularRateMeasX2 = biasX + m1.getElementAtIndex(0);
final var angularRateMeasY2 = biasY + m1.getElementAtIndex(1);
final var angularRateMeasZ2 = biasZ + m1.getElementAtIndex(2);
final var diffX = angularRateMeasX2 - angularRateMeasX1; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 94 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 98 |
KnownBiasGyroscopeCalibrator, OrderedStandardDeviationBodyKinematicsGyroscopeCalibrator,
QualityScoredGyroscopeCalibrator, AccelerometerDependentGyroscopeCalibrator {
/**
* Indicates whether by default a common z-axis is assumed for both the accelerometer
* and gyroscope.
*/
public static final boolean DEFAULT_USE_COMMON_Z_AXIS = true;
/**
* Indicates that by default G-dependent cross biases introduced
* by the accelerometer on the gyroscope are estimated.
*/
public static final boolean DEFAULT_ESTIMATE_G_DEPENDENT_CROSS_BIASES = true;
/**
* Default turntable rotation rate.
*/
public static final double DEFAULT_TURNTABLE_ROTATION_RATE =
TurntableGyroscopeCalibrator.DEFAULT_TURNTABLE_ROTATION_RATE;
/**
* Default time interval between measurements expressed in seconds (s).
* This is a typical value when we have 50 samples per second.
*/
public static final double DEFAULT_TIME_INTERVAL = TurntableGyroscopeCalibrator.DEFAULT_TIME_INTERVAL;
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default using a non-linear calibrator
* (which uses a Levenberg-Marquardt fitter).
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Known x-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasX;
/**
* Known y-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasY;
/**
* Known z-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasZ;
/**
* Known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSx;
/**
* Known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSy;
/**
* Known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSz;
/**
* Known accelerometer x-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxy;
/**
* Know accelerometer x-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxz;
/**
* Known accelerometer y-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyx;
/**
* Known accelerometer y-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyz;
/**
* Known accelerometer z-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzx;
/**
* Known accelerometer z-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzy;
/**
* Known x-coordinate of gyroscope bias.
* This is expressed in radians per second (rad/s).
*/
private double biasX; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5445 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8760 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6482 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7631 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 3039 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 10944 |
final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
m1.add(mg);
final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
angularRateTrue.setElementAtIndex(0, angularRateTrueX);
angularRateTrue.setElementAtIndex(1, angularRateTrueY);
angularRateTrue.setElementAtIndex(2, angularRateTrueZ);
m1.multiply(angularRateTrue);
final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
fTrue.setElementAtIndex(0, fTrueX);
fTrue.setElementAtIndex(1, fTrueY);
fTrue.setElementAtIndex(2, fTrueZ);
final var m2 = gg.multiplyAndReturnNew(fTrue);
m1.add(m2);
final var angularRateMeasX2 = bx + m1.getElementAtIndex(0);
final var angularRateMeasY2 = by + m1.getElementAtIndex(1);
final var angularRateMeasZ2 = bz + m1.getElementAtIndex(2);
final var diffX = angularRateMeasX2 - angularRateMeasX1; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5963 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9217 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6980 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8107 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1288 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2364 |
}
/**
* Fills scale factor and cross coupling error matrix with estimated values.
*
* @param sx x scale factor
* @param sy y scale factor
* @param sz z scale factor
* @param mxy x-y cross coupling
* @param mxz x-z cross coupling
* @param myx y-x cross coupling
* @param myz y-z cross coupling
* @param mzx z-x cross coupling
* @param mzy z-y cross coupling
* @throws WrongSizeException never happens.
*/
private void fillMm(final double sx, final double sy, final double sz,
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy) throws WrongSizeException {
if (estimatedMm == null) {
estimatedMm = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
}
estimatedMm.setElementAt(0, 0, sx);
estimatedMm.setElementAt(1, 0, myx);
estimatedMm.setElementAt(2, 0, mzx);
estimatedMm.setElementAt(0, 1, mxy);
estimatedMm.setElementAt(1, 1, sy);
estimatedMm.setElementAt(2, 1, mzy);
estimatedMm.setElementAt(0, 2, mxz);
estimatedMm.setElementAt(1, 2, myz);
estimatedMm.setElementAt(2, 2, sz);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 5821 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 5571 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 6365 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 6088 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10518 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 11020 |
result.estimatedMa = innerCalibrator.getEstimatedMa();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierMeasurements.add(measurements.get(i));
}
}
try {
innerCalibrator.setBiasCoordinates(biasX, biasY, biasZ); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4999 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3222 |
listener.onCalibrateEnd((C) this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 101 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 129 |
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default using a non-linear calibrator
* (which uses a Levenberg-Marquardt fitter).
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Known x-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasX;
/**
* Known y-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasY;
/**
* Known z-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasZ;
/**
* Known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSx;
/**
* Known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSy;
/**
* Known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSz;
/**
* Known accelerometer x-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxy;
/**
* Know accelerometer x-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxz;
/**
* Known accelerometer y-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyx;
/**
* Known accelerometer y-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyz;
/**
* Known accelerometer z-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzx;
/**
* Known accelerometer z-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzy;
/**
* Initial x-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*/
private double initialBiasX;
/**
* Initial y-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*/
private double initialBiasY;
/**
* Initial z-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*/
private double initialBiasZ;
/**
* Initial gyroscope x scaling factor.
*/
private double initialSx;
/**
* Initial gyroscope y scaling factor.
*/
private double initialSy;
/**
* Initial gyroscope z scaling factor.
*/
private double initialSz;
/**
* Initial gyroscope x-y cross coupling error.
*/
private double initialMxy;
/**
* Initial gyroscope x-z cross coupling error.
*/
private double initialMxz;
/**
* Initial gyroscope y-x cross coupling error.
*/
private double initialMyx;
/**
* Initial gyroscope y-z cross coupling error.
*/
private double initialMyz;
/**
* Initial gyroscope z-x cross coupling error.
*/
private double initialMzx;
/**
* Initial gyroscope z-y cross coupling error.
*/
private double initialMzy;
/**
* Initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*/
private Matrix initialGg; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 5212 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 2988 |
return create(measurements, bias, commonAxisUsed, listener, DEFAULT_ROBUST_METHOD);
}
/**
* Computes error of a preliminary result respect a given measurement.
*
* @param measurement a measurement.
* @param preliminaryResult a preliminary result.
* @return computed error.
*/
protected double computeError(
final StandardDeviationFrameBodyKinematics measurement, final PreliminaryResult preliminaryResult) {
// We know that measured angular rate is:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
// Hence:
// [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1 0] [myx sy myz] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1] [mzx mzy sz ] [Ωtruez] [g31 g32 g33][ftruez]
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
final var expectedKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
previousEcefFrame);
final var angularRateMeasX1 = measuredKinematics.getAngularRateX();
final var angularRateMeasY1 = measuredKinematics.getAngularRateY();
final var angularRateMeasZ1 = measuredKinematics.getAngularRateZ();
final var angularRateTrueX = expectedKinematics.getAngularRateX();
final var angularRateTrueY = expectedKinematics.getAngularRateY();
final var angularRateTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
final var mg = preliminaryResult.estimatedMg; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 96 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 124 |
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default using a non-linear calibrator
* (which uses a Levenberg-Marquardt fitter).
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Known x-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasX;
/**
* Known y-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasY;
/**
* Known z-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasZ;
/**
* Known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSx;
/**
* Known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSy;
/**
* Known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSz;
/**
* Known accelerometer x-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxy;
/**
* Know accelerometer x-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxz;
/**
* Known accelerometer y-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyx;
/**
* Known accelerometer y-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyz;
/**
* Known accelerometer z-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzx;
/**
* Known accelerometer z-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzy;
/**
* X-coordinate of gyroscope known bias expressed in radians per second
* (rad/s).
*/
private double biasX;
/**
* Y-coordinate of gyroscope known bias expressed in radians per second
* (rad/s).
*/
private double biasY;
/**
* Z-coordinate of gyroscope known bias expressed in radians per second
* (rad/s).
*/
private double biasZ;
/**
* Initial gyroscope x scaling factor.
*/
private double initialSx;
/**
* Initial gyroscope y scaling factor.
*/
private double initialSy;
/**
* Initial gyroscope z scaling factor.
*/
private double initialSz;
/**
* Initial gyroscope x-y cross coupling error.
*/
private double initialMxy;
/**
* Initial gyroscope x-z cross coupling error.
*/
private double initialMxz;
/**
* Initial gyroscope y-x cross coupling error.
*/
private double initialMyx;
/**
* Initial gyroscope y-z cross coupling error.
*/
private double initialMyz;
/**
* Initial gyroscope z-x cross coupling error.
*/
private double initialMzx;
/**
* Initial gyroscope z-y cross coupling error.
*/
private double initialMzy;
/**
* Initial G-dependent cross biases introduced on the gyroscope by the
* specific forces sensed by the accelerometer.
*/
private Matrix initialGg; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6813 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8077 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6949 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7941 |
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7304 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8542 |
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7434 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8412 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4225 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4338 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3735 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3848 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5216 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7386 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6253 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8514 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6631 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7877 |
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6748 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7759 |
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5735 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7863 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6752 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8973 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7124 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8348 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7240 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8232 |
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1089 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1093 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether estimator is ready to start the estimator.
*
* @return true if estimator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether estimator is currently running or not.
*
* @return true if estimator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if estimator is currently running.
* @throws NotReadyException if estimator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5102 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5227 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustEasyGyroscopeCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4622 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4747 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/NEDPositionEstimator.java | 2009 |
| com/irurueta/navigation/inertial/estimators/NEDVelocityEstimator.java | 2306 |
estimatePosition(timeInterval, oldPosition, oldVelocity, velocity, result);
return result;
}
/**
* Converts provided time instance to seconds (s).
*
* @param time time instance to be converted.
* @return provided time value expressed in seconds.
*/
private static double convertTimeToDouble(final Time time) {
return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
}
/**
* Converts provided angle instance to radians (rad).
*
* @param angle angle instance to be converted.
* @return provided angle value expressed in radians.
*/
private static double convertAngleToDouble(final Angle angle) {
return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
}
/**
* Converts provided distance instance to meters (m).
*
* @param distance distance instance to be converted.
* @return provided distance value expressed in meters.
*/
private static double convertDistanceToDouble(final Distance distance) {
return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
}
/**
* Converts provided speed instance to meters per second (m/s).
*
* @param speed speed instance to be converted.
* @return provided speed value expressed in meters per second.
*/
private static double convertSpeedToDouble(final Speed speed) {
return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/AccelerationFixer.java | 1171 |
| com/irurueta/navigation/inertial/calibration/MagneticFluxDensityFixer.java | 1253 |
final double measuredFx, final double measuredFy, final double measuredFz,
final double biasX, final double biasY, final double biasZ,
final double sx, final double sy, final double sz, final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy, final double[] result) throws AlgebraException {
crossCouplingErrors.setElementAt(0, 0, sx);
crossCouplingErrors.setElementAt(1, 1, sy);
crossCouplingErrors.setElementAt(2, 2, sz);
crossCouplingErrors.setElementAt(0, 1, mxy);
crossCouplingErrors.setElementAt(0, 2, mxz);
crossCouplingErrors.setElementAt(1, 0, myx);
crossCouplingErrors.setElementAt(1, 2, myz);
crossCouplingErrors.setElementAt(2, 0, mzx);
crossCouplingErrors.setElementAt(2, 1, mzy);
fix(measuredFx, measuredFy, measuredFz, biasX, biasY, biasZ, crossCouplingErrors, result); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1089 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 421 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether estimator is ready to start the estimator.
*
* @return true if estimator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether estimator is currently running or not.
*
* @return true if estimator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if estimator is currently running.
* @throws NotReadyException if estimator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets estimated accelerometer scale factors and ross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*
* @return estimated accelerometer scale factors and cross coupling errors, or null
* if not available.
*/
@Override
public Matrix getEstimatedMa() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 409 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1093 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the calibration.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4127 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5535 |
return convertAngularSpeed(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit());
}
/**
* Makes proper conversion of internal cross-coupling, bias and g-dependent
* cross bias matrices.
*
* @param m internal scaling and cross-coupling matrix.
* @param g internal g-dependent cross bias matrix.
* @throws AlgebraException if a numerical instability occurs.
*/
private void setResult(final Matrix m, final Matrix g) throws AlgebraException {
setResult(m);
// Gg = M*G
m.multiply(g, estimatedGg);
}
/**
* Makes proper conversion of internal cross-coupling and bias matrices.
*
* @param m internal scaling and cross-coupling matrix.
* @throws AlgebraException if a numerical instability occurs.
*/
private void setResult(final Matrix m) throws AlgebraException {
// Because:
// M = I + Mg
// b = M^-1*bg
// Then:
// Mg = M - I
// bg = M*b
if (estimatedMg == null) {
estimatedMg = m;
} else {
estimatedMg.copyFrom(m);
}
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
estimatedMg.setElementAt(i, i, estimatedMg.getElementAt(i, i) - 1.0);
}
if (estimatedGg == null) {
estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedGg.initialize(0.0);
}
estimatedCovariance = fitter.getCovar();
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Computes gravity versor error at the end of a sequence using provided
* parameters.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param i row position.
* @param params array containing parameters for the general purpose case
* when G-dependent cross biases are taken into account. Must
* have length 18.
* @return error between estimated and measured gravity versor.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5216 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8514 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6253 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7386 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5735 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8973 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6752 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7863 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4574 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5610 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5109 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6126 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 332 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 331 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1050 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1049 |
setupWmmEstimator();
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.LMEDS;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5789 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5895 |
final var initialB = invInitialM.multiplyAndReturnNew(initialBa);
fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
@Override
public int getNumberOfDimensions() {
// Input points are measured specific force coordinates
return BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[GENERAL_UNKNOWNS];
// biases b
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
initial[i] = initialB.getElementAtIndex(i);
}
// cross coupling errors M
final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
for (int i = 0, j = BodyKinematics.COMPONENTS; i < num; i++, j++) {
initial[j] = initialM.getElementAtIndex(i);
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 703 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 711 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2651 |
final KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return x coordinate of accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasX x coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return y coordinate of accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasY y coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return z coordinate of accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasZ z coordinate of accelerometer bias.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x coordinate of accelerometer bias.
*
* @return x coordinate of accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/generators/AccelerometerAndGyroscopeMeasurementsGenerator.java | 738 |
| com/irurueta/navigation/inertial/calibration/generators/AccelerometerGyroscopeAndMagnetometerMeasurementsGenerator.java | 829 |
gyroscopeMeasurementsGenerator.reset();
if (listener != null) {
listener.onReset(this);
}
}
/**
* Gets estimated average angular rate during initialization phase.
*
* @return estimated average angular rate during initialization phase.
*/
public AngularSpeedTriad getInitialAvgAngularSpeedTriad() {
return gyroscopeMeasurementsGenerator.getInitialAvgAngularSpeedTriad();
}
/**
* Gets estimated average angular rate during initialization phase.
*
* @param result instance where result will be stored.
*/
public void getInitialAvgAngularSpeedTriad(final AngularSpeedTriad result) {
gyroscopeMeasurementsGenerator.getInitialAvgAngularSpeedTriad(result);
}
/**
* Gets estimated standard deviation of angular rate during initialization phase.
*
* @return estimated standard deviation of angular rate during initialization phase.
*/
public AngularSpeedTriad getInitialAngularSpeedTriadStandardDeviation() {
return gyroscopeMeasurementsGenerator.getInitialAngularSpeedTriadStandardDeviation();
}
/**
* Gets estimated standard deviation of angular rate during initialization phase.
*
* @param result instance where result will be stored.
*/
public void getInitialAngularSpeedTriadStandardDeviation(final AngularSpeedTriad result) {
gyroscopeMeasurementsGenerator.getInitialAngularSpeedTriadStandardDeviation(result);
}
/**
* Gets gyroscope base noise level that has been detected during
* initialization expressed in radians per second (rad/s).
* This is equal to the standard deviation of the gyroscope measurements
* during initialization phase.
*
* @return gyroscope base noise level.
*/
public double getGyroscopeBaseNoiseLevel() {
return gyroscopeMeasurementsGenerator.getGyroscopeBaseNoiseLevel();
}
/**
* Gets gyroscope base noise level that has been detected during
* initialization.
* This is equal to the standard deviation of the gyroscope measurements
* during initialization phase.
*
* @return gyroscope base noise level.
*/
public AngularSpeed getGyroscopeBaseNoiseLevelAsMeasurement() {
return gyroscopeMeasurementsGenerator.getGyroscopeBaseNoiseLevelAsMeasurement();
}
/**
* Gets gyroscope base noise level that has been detected during
* initialization.
* This is equal to the standard deviation of the gyroscope measurements
* during initialization phase.
*
* @param result instance where result will be stored.
*/
public void getGyroscopeBaseNoiseLevelAsMeasurement(final AngularSpeed result) {
gyroscopeMeasurementsGenerator.getGyroscopeBaseNoiseLevelAsMeasurement(result);
}
/**
* Gets gyroscope base noise level PSD (Power Spectral Density)
* expressed in (rad^2/s).
*
* @return gyroscope base noise level PSD.
*/
public double getGyroscopeBaseNoiseLevelPsd() {
return gyroscopeMeasurementsGenerator.getGyroscopeBaseNoiseLevelPsd();
}
/**
* Gets gyroscope base noise level root PSD (Power Spectral Density)
* expressed in (rad * s^-0.5)
*
* @return gyroscope base noise level root PSD.
*/
@Override
public double getGyroscopeBaseNoiseLevelRootPsd() {
return gyroscopeMeasurementsGenerator.getGyroscopeBaseNoiseLevelRootPsd();
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4282 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4395 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4688 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4784 |
final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must be 3x1
* and is expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3792 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3905 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This
* must have length 3 and is
* expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors
* and cross coupling matrix. Must
* be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4204 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4302 |
final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
qualityScores, sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
qualityScores, sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must be 3x1
* and is expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4939 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6111 |
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity and
* unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross
* biases will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events
* raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5074 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5976 |
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param bias known gyroscope bias. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors
* matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5370 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7551 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6407 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8680 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10465 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 10964 |
final var angularRateMeasZ2 = biasZ + m1.getElementAtIndex(2);
final var sqrNormMeas1 = angularRateMeasX1 * angularRateMeasX1 + angularRateMeasY1 * angularRateMeasY1
+ angularRateMeasZ1 * angularRateMeasZ1;
final var sqrNormMeas2 = angularRateMeasX2 * angularRateMeasX2 + angularRateMeasY2 * angularRateMeasY2
+ angularRateMeasZ2 * angularRateMeasZ2;
final var normMeas1 = Math.sqrt(sqrNormMeas1);
final var normMeas2 = Math.sqrt(sqrNormMeas2);
return Math.abs(normMeas1 - normMeas2);
} catch (final WrongSizeException | InvalidRotationMatrixException
| InvalidSourceAndDestinationFrameTypeException e) {
return Double.MAX_VALUE;
}
}
/**
* Computes a preliminary solution for a subset of samples picked by a robust estimator.
*
* @param samplesIndices indices of samples picked by the robust estimator.
* @param solutions list where estimated preliminary solution will be stored.
*/
protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {
final var meas = new ArrayList<StandardDeviationBodyKinematics>();
for (final var samplesIndex : samplesIndices) {
meas.add(this.measurements.get(samplesIndex));
}
try {
final var result = new PreliminaryResult();
result.estimatedMg = getInitialMg(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5463 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6613 |
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must be 3x1 and is expressed in
* radians per second (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific
* forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5596 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6480 |
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics
* measures have been taken.
* @param turntableRotationRate constant rotation rate at which
* the turntable is spinning. Must
* be expressed in radians per
* second (rad/s).
* @param timeInterval time interval between measurements
* being captured expressed in
* seconds (s).
* @param measurements collection of body kinematics
* measurements with standard
* deviations taken at the same
* position with zero velocity
* and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be
* used to find a solution. This
* must have length 3 and is
* expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors
* and cross coupling errors matrix.
* Must be 3x3.
* @param initialGg initial gyroscope G-dependent
* cross biases introduced on the
* gyroscope by the specific forces
* sensed by the accelerometer.
* Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5887 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8026 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6904 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9136 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/noise/WindowedBodyKinematicsNoiseEstimator.java | 220 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java | 197 |
if (windowSize < MIN_WINDOW_SIZE) {
throw new IllegalArgumentException();
}
this.windowSize = windowSize;
reset();
}
/**
* Gets time interval between body kinematics samples expressed in
* seconds (s).
*
* @return time interval between accelerometer triad samples.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between body kinematics samples expressed in
* seconds (s).
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval < 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between body kinematics samples.
*
* @return time interval between accelerometer triad samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between body kinematics samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between body kinematics samples.
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
TimeUnit.SECOND));
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public WindowedBodyKinematicsNoiseEstimatorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4471 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5507 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4472 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4575 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5508 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5611 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5004 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6021 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5005 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5110 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6022 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6127 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised
* by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 13635 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 6392 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 22417 |
return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
}
/**
* Converts provided distance instance into its corresponding value expressed in
* meters.
*
* @param distance distance instance to be converted.
* @return converted value expressed in meters.
*/
private static double convertDistanceToDouble(final Distance distance) {
return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
}
/**
* Converts provided speed instance into its corresponding value expressed in
* meters per second.
*
* @param speed speed instance to be converted.
* @return converted value expressed in meters per second.
*/
private static double convertSpeedToDouble(final Speed speed) {
return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
}
/**
* Converts provided acceleration instance into its corresponding value expressed
* in meters per squared second.
*
* @param acceleration acceleration instance to be converted.
* @return converted value expressed in meters per squared second.
*/
private static double convertAccelerationToDouble(final Acceleration acceleration) {
return AccelerationConverter.convert(acceleration.getValue().doubleValue(), acceleration.getUnit(),
AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Converts provided angular speed into its corresponding value expressed in
* radians per second.
*
* @param angularSpeed angular speed instance to be converted.
* @return converted value expressed in radians per second.
*/
private static double convertAngularSpeedToDouble(final AngularSpeed angularSpeed) {
return AngularSpeedConverter.convert(angularSpeed.getValue().doubleValue(), angularSpeed.getUnit(),
AngularSpeedUnit.RADIANS_PER_SECOND);
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6361 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6573 |
fmeas.setElementAtIndex(0, fmeasX);
fmeas.setElementAtIndex(1, fmeasY);
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, bx);
b.setElementAtIndex(1, by);
b.setElementAtIndex(2, bz); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3714 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4460 |
initial[3] = initialSx;
initial[4] = initialSy;
initial[5] = initialSz;
initial[6] = initialMxy;
initial[7] = initialMxz;
initial[8] = initialMyz;
return initial;
}
@Override
public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
final Matrix jacobian) {
// We know that:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + ftruez + sz * ftruez
// Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
// sz, mxy, mxz, myz
// d(fmeasx)/d(bx) = 1.0
// d(fmeasx)/d(by) = 0.0
// d(fmeasx)/d(bz) = 0.0
// d(fmeasx)/d(sx) = ftruex
// d(fmeasx)/d(sy) = 0.0
// d(fmeasx)/d(sz) = 0.0
// d(fmeasx)/d(mxy) = ftruey
// d(fmeasx)/d(mxz) = ftruez
// d(fmeasx)/d(myz) = 0.0
// d(fmeasy)/d(bx) = 0.0
// d(fmeasy)/d(by) = 1.0
// d(fmeasy)/d(bz) = 0.0
// d(fmeasy)/d(sx) = 0.0
// d(fmeasy)/d(sy) = ftruey
// d(fmeasy)/d(sz) = 0.0
// d(fmeasy)/d(mxy) = 0.0
// d(fmeasy)/d(mxz) = 0.0
// d(fmeasy)/d(myz) = ftruez
// d(fmeasz)/d(bx) = 0.0
// d(fmeasz)/d(by) = 0.0
// d(fmeasz)/d(bz) = 1.0
// d(fmeasz)/d(sx) = 0.0
// d(fmeasz)/d(sy) = 0.0
// d(fmeasz)/d(sz) = ftruez
// d(fmeasz)/d(mxy) = 0.0
// d(fmeasz)/d(mxz) = 0.0
// d(fmeasz)/d(myz) = 0.0
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var sx = params[3];
final var sy = params[4];
final var sz = params[5];
final var mxy = params[6];
final var mxz = params[7];
final var myz = params[8];
final var ftruex = point[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 885 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 256 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 893 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 779 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 1712 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 261 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java | 1745 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 258 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 973 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 971 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 975 |
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4710 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4005 |
jacobian.setElementAt(11, 8, 1.9);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Sets input data into Levenberg-Marquardt fitter.
*
* @throws AlgebraException if there are numerical instabilities.
*/
private void setInputData() throws AlgebraException {
final var ba = getAccelerometerBiasAsMatrix();
final var ma = getAccelerometerMa();
final var measuredBeforeF = new double[BodyKinematics.COMPONENTS];
final var fixedBeforeF = new double[BodyKinematics.COMPONENTS];
final var measuredAfterF = new double[BodyKinematics.COMPONENTS];
final var fixedAfterF = new double[BodyKinematics.COMPONENTS];
final var numSequences = sequences.size();
final var x = new Matrix(numSequences, 2 * BodyKinematics.COMPONENTS);
final var y = new double[numSequences];
final var standardDeviations = new double[numSequences];
// make a copy of input sequences that will be used to update
// kinematics measurements with fixed values for memory efficiency
fixedSequences = new ArrayList<>();
for (final var sequence : sequences) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4506 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4589 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
initialGg, listener);
default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4833 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4932 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4019 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4105 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
qualityScores, sequences, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
qualityScores, sequences, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4353 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4452 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(qualityScores, sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5370 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8680 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6407 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7551 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5887 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9136 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6904 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8026 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6045 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4275 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5639 |
return BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[COMMON_Z_AXIS_UNKNOWNS];
// biases b
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
initial[i] = initialB.getElementAtIndex(i);
}
// upper diagonal cross coupling errors M
var k = BodyKinematics.COMPONENTS;
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 552 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 552 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1075 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1074 |
setupWmmEstimator();
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2157 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2156 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 356 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 355 |
initialize();
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROSAC; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/NEDPositionEstimator.java | 2011 |
| com/irurueta/navigation/inertial/estimators/NEDVelocityEstimator.java | 2308 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 22396 |
}
/**
* Converts provided time instance to seconds (s).
*
* @param time time instance to be converted.
* @return provided time value expressed in seconds.
*/
private static double convertTimeToDouble(final Time time) {
return TimeConverter.convert(time.getValue().doubleValue(), time.getUnit(), TimeUnit.SECOND);
}
/**
* Converts provided angle instance to radians (rad).
*
* @param angle angle instance to be converted.
* @return provided angle value expressed in radians.
*/
private static double convertAngleToDouble(final Angle angle) {
return AngleConverter.convert(angle.getValue().doubleValue(), angle.getUnit(), AngleUnit.RADIANS);
}
/**
* Converts provided distance instance to meters (m).
*
* @param distance distance instance to be converted.
* @return provided distance value expressed in meters.
*/
private static double convertDistanceToDouble(final Distance distance) {
return DistanceConverter.convert(distance.getValue().doubleValue(), distance.getUnit(), DistanceUnit.METER);
}
/**
* Converts provided speed instance to meters per second (m/s).
*
* @param speed speed instance to be converted.
* @return provided speed value expressed in meters per second.
*/
private static double convertSpeedToDouble(final Speed speed) {
return SpeedConverter.convert(speed.getValue().doubleValue(), speed.getUnit(), SpeedUnit.METERS_PER_SECOND);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5002 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2977 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2865 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2995 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3923 |
return BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[COMMON_Z_AXIS_UNKNOWNS];
initial[0] = initialSx;
initial[1] = initialSy;
initial[2] = initialSz;
initial[3] = initialMxy;
initial[4] = initialMxz;
initial[5] = initialMyz;
return initial;
}
@Override
public void evaluate(
final int i, final double[] point, final double[] result,
final double[] params, final Matrix jacobian) {
// We know that
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + ftruez + sz * ftruez
// Hence, the derivatives respect the parameters sx, sy, sz, mxy, mxz, myz are:
// d(fmeasx)/d(sx) = ftruex
// d(fmeasx)/d(sy) = 0.0
// d(fmeasx)/d(sz) = 0.0
// d(fmeasx)/d(mxy) = ftruey
// d(fmeasx)/d(mxz) = ftruez
// d(fmeasx)/d(myz) = 0.0
// d(fmeasy)/d(sx) = 0.0
// d(fmeasy)/d(sy) = ftruey
// d(fmeasy)/d(sz) = 0.0
// d(fmeasy)/d(mxy) = 0.0
// d(fmeasy)/d(mxz) = 0.0
// d(fmeasy)/d(myz) = ftruez
// d(fmeasy)/d(sx) = 0.0
// d(fmeasy)/d(sy) = 0.0
// d(fmeasy)/d(sz) = ftruez
// d(fmeasy)/d(mxy) = 0.0
// d(fmeasy)/d(mxz) = 0.0
// d(fmeasy)/d(myz) = 0.0
final var sx = params[0];
final var sy = params[1];
final var sz = params[2];
final var mxy = params[3];
final var mxz = params[4];
final var myz = params[5];
final var ftruex = point[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4685 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5847 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4810 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5721 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5222 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6358 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5341 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6239 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 571 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 263 |
this(measurements, biasX, biasY, biasZ, commonAxisUsed);
this.listener = listener;
}
/**
* Gets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<FrameBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @param measurements collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* @throws LockedException if estimator is currently running.
*/
@Override
public void setMeasurements(final Collection<? extends FrameBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
//noinspection unchecked
this.measurements = (Collection<FrameBodyKinematics>) measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public AccelerometerCalibratorMeasurementType getMeasurementType() {
return AccelerometerCalibratorMeasurementType.FRAME_BODY_KINEMATICS;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
@Override
public KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 580 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 275 |
this(measurements, biasX, biasY, biasZ, commonAxisUsed);
this.listener = listener;
}
/**
* Gets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<FrameBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @param measurements collection of body kinematics measurements taken at different
* frames (positions, orientations and velocities).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final Collection<? extends FrameBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
//noinspection unchecked
this.measurements = (Collection<FrameBodyKinematics>) measurements;
}
/**
* Indicates the type of measurement or sequence used by this calibrator.
*
* @return type of measurement or sequence used by this calibrator.
*/
@Override
public GyroscopeCalibratorMeasurementOrSequenceType getMeasurementOrSequenceType() {
return GyroscopeCalibratorMeasurementOrSequenceType.FRAME_BODY_KINEMATICS_MEASUREMENT;
}
/**
* Indicates whether this calibrator requires ordered measurements or sequences
* in a list or not.
*
* @return true if measurements or sequences must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsOrSequencesRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement/sequence or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mg matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mg matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this calibrator.
*
* @return listener to handle events raised by this calibrator.
*/
@Override
public KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 596 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1672 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 580 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1640 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates gyroscope calibration parameters containing scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 285 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 284 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1001 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1002 |
setupWmmEstimator();
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.MSAC;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 5111 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6059 |
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
return initial;
}
@Override
public double evaluate(final int i, final double[] point, final double[] params,
final double[] derivatives) throws EvaluationException {
fmeasX = point[0];
fmeasY = point[1];
fmeasZ = point[2];
gradientEstimator.gradient(params, derivatives);
return evaluateCommonAxis(params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var m11 = result[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1003 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1029 |
final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
this(measurements, commonAxisUsed, listener);
internalSetBias(bias);
}
/**
* Gets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return x coordinate of accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasX x coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return y coordinate of accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasY y coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return z coordinate of accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasZ z coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x coordinate of accelerometer bias.
*
* @return x coordinate of accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1371 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1502 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3962 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 909 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 1050 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4666 |
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var omegaMeasX = measuredKinematics.getAngularRateX();
final var omegaMeasY = measuredKinematics.getAngularRateY();
final var omegaMeasZ = measuredKinematics.getAngularRateZ();
final var omegaTrueX = expectedKinematics.getAngularRateX();
final var omegaTrueY = expectedKinematics.getAngularRateY();
final var omegaTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5016 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5653 |
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException {
measAngularRateX = point[0];
measAngularRateY = point[1];
measAngularRateZ = point[2];
gradientEstimator.gradient(params, derivatives);
return evaluateCommonAxis(params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var m11 = result[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4524 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5663 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4627 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5560 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4746 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6879 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5782 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8007 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5058 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6180 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5163 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6075 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have length
* 3 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5280 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7366 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6297 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8474 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2490 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3196 |
for (var j = 0; j < BodyMagneticFluxDensity.COMPONENTS; j++) {
for (var i = 0; i < BodyMagneticFluxDensity.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException {
bmeasX = point[0];
bmeasY = point[1];
bmeasZ = point[2];
gradientEstimator.gradient(params, derivatives);
return evaluateCommonAxis(params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var m11 = result[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 468 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 496 |
final RobustKnownFrameAccelerometerCalibratorListener listener) {
this(measurements, commonAxisUsed);
this.listener = listener;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solutions.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2) and only taken into
* account if non-linear preliminary solutions are used.
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is only taken into account if non-linear preliminary solutions are used.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasXAsAcceleration() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5929 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2959 |
measAngularRateZ = point[2];
gradientEstimator.gradient(params, derivatives);
return evaluateGeneral(params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var m11 = result[3];
final var m21 = result[4];
final var m31 = result[5];
final var m12 = result[6];
final var m22 = result[7];
final var m32 = result[8];
final var m13 = result[9];
final var m23 = result[10];
final var m33 = result[11];
final var mb = new Matrix(BodyKinematics.COMPONENTS, 1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 5300 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6336 |
private double evaluate(final double m11, final double m21, final double m31,
final double m12, final double m22, final double m32,
final double m13, final double m23, final double m33) throws EvaluationException {
// fmeas = M*(ftrue + b)
// ftrue = M^-1*fmeas - b
try {
if (fmeas == null) {
fmeas = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (m == null) {
m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (invM == null) {
invM = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (b == null) {
b = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (ftrue == null) {
ftrue = new Matrix(BodyKinematics.COMPONENTS, 1);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1676 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1576 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1695 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1594 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/bias/BodyKinematicsBiasEstimator.java | 1246 |
| com/irurueta/navigation/inertial/calibration/bias/BodyMagneticFluxDensityBiasEstimator.java | 1873 |
rebuildExpectedKinematics();
}
/**
* Gets ECEF frame containing current body position and orientation expressed
* in ECEF coordinates. Frame also contains body velocity, but it is always
* assumed to be zero during calibration.
*
* @return ECEF frame containing current body position and orientation resolved
* around ECEF axes.
*/
public ECEFFrame getEcefFrame() {
return new ECEFFrame(frame);
}
/**
* Gets ECEF frame containing current body position and orientation expressed
* in ECEF coordinates. Frame also contains body velocity, but it is always
* assumed to be zero during calibration.
*
* @param result instance where ECEF frame containing current body position and
* orientation resolved around ECEF axes will be stored.
*/
public void getEcefFrame(final ECEFFrame result) {
frame.copyTo(result);
}
/**
* Gets NED frame containing current body position and orientation expressed
* in NED coordinates. Frame also contains body velocity, but it is always
* assumed to be zero during calibration.
*
* @return NED frame containing current body position and orientation resolved
* around NED axes.
*/
public NEDFrame getNedFrame() {
return ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame);
}
/**
* Gets NED frame containing current body position and orientation expressed
* in NED coordinates. Frame also contains body velocity, but it is always
* assumed to be zero during calibration.
*
* @param result instance where NED frame containing current body position and
* orientation resolved around NED axes will be stored.
*/
public void getNedFrame(final NEDFrame result) {
ECEFtoNEDFrameConverter.convertECEFtoNED(frame, result);
}
/**
* Gets current body position expressed in NED coordinates.
*
* @return current body position expressed in NED coordinates.
*/
public NEDPosition getNedPosition() {
return getNedFrame().getPosition();
}
/**
* Gets current body position expressed in NED coordinates.
*
* @param result instance where current body position will be stored.
*/
public void getNedPosition(final NEDPosition result) {
getNedFrame().getPosition(result);
}
/**
* Sets current body position expressed in NED coordinates.
*
* @param position current body position to be set.
* @throws LockedException if estimator is currently running.
*/
public void setNedPosition(final NEDPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
final var nedFrame = getNedFrame();
nedFrame.setPosition(position);
NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 5119 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4379 |
b = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (g == null) {
g = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp == null) {
tmp = new Matrix(BodyKinematics.COMPONENTS, 1);
}
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3546 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3914 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4206 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4618 |
estimatedMg.setElementAt(1, 1, sy);
estimatedMg.setElementAt(0, 2, mxz);
estimatedMg.setElementAt(1, 2, myz);
estimatedMg.setElementAt(2, 2, sz);
if (estimatedGg == null) {
estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedGg.initialize(0.0);
}
estimatedGg.setElementAtIndex(0, g11);
estimatedGg.setElementAtIndex(1, g21);
estimatedGg.setElementAtIndex(2, g31);
estimatedGg.setElementAtIndex(3, g12);
estimatedGg.setElementAtIndex(4, g22);
estimatedGg.setElementAtIndex(5, g32);
estimatedGg.setElementAtIndex(6, g13);
estimatedGg.setElementAtIndex(7, g23);
estimatedGg.setElementAtIndex(8, g33);
estimatedCovariance = fitter.getCovar(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4746 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8007 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5141 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7306 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5782 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6879 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6178 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8434 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5280 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8474 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5659 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7782 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6297 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7366 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6676 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8892 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2685 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3473 |
final double m11, final double m21, final double m31,
final double m12, final double m22, final double m32,
final double m13, final double m23, final double m33) throws EvaluationException {
// bmeas = M*(btrue + b)
// btrue = M^-1*bmeas - b
try {
if (bmeas == null) {
bmeas = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
}
if (m == null) {
m = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
}
if (invM == null) {
invM = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
}
if (b == null) {
b = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
}
if (btrue == null) {
btrue = new Matrix(BodyMagneticFluxDensity.COMPONENTS, 1);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1944 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1955 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java | 1219 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3463 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 1189 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3398 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1215 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3458 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sequence.
* The larger the score value the better the quality of the sequence.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sequence.
* The larger the score value the better the quality of the sequence.
*
* @param qualityScores quality scores corresponding to each sequence.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1185 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3395 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sequence.
* The larger the score value the better the quality of the sequence.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sequence.
* The larger the score value the better the quality of the sequence.
*
* @param qualityScores quality scores corresponding to each sequence.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4521 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4856 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1911 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1993 |
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public L getListener() {
return listener;
}
/**
* Sets listener to handle events raised by this estimator.
*
* @param listener listener to handle events raised by this estimator.
* @throws LockedException if calibrator is currently running.
*/
public void setListener(final L listener) throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return commonAxisUsed ? MINIMUM_MEASUREMENTS_COMMON_Z_AXIS : MINIMUM_MEASUREMENTS_GENERAL;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= getMinimumRequiredMeasurements() | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 82 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 78 |
UnknownBiasGyroscopeCalibrator, GyroscopeCalibrationSource, GyroscopeBiasUncertaintySource,
OrderedBodyKinematicsSequenceGyroscopeCalibrator, QualityScoredGyroscopeCalibrator,
AccelerometerDependentGyroscopeCalibrator {
/**
* Indicates whether by default a common z-axis is assumed for both the accelerometer
* and gyroscope.
*/
public static final boolean DEFAULT_USE_COMMON_Z_AXIS = true;
/**
* Indicates that by default G-dependent cross biases introduced
* by the accelerometer on the gyroscope are estimated.
*/
public static final boolean DEFAULT_ESTIMATE_G_DEPENDENT_CROSS_BIASES = true;
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default using a non-linear calibrator
* (which uses a Levenberg-Marquardt fitter).
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Known x-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasX;
/**
* Known y-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasY;
/**
* Known z-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasZ;
/**
* Known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSx;
/**
* Known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSy;
/**
* Known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSz;
/**
* Known accelerometer x-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxy;
/**
* Know accelerometer x-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxz;
/**
* Known accelerometer y-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyx;
/**
* Known accelerometer y-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyz;
/**
* Known accelerometer z-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzx;
/**
* Known accelerometer z-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzy;
/**
* Initial x-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*/
private double initialBiasX; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3916 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4000 |
final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must be 3x1
* and is expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 5252 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 3034 |
final var mg = preliminaryResult.estimatedMg;
final var gg = preliminaryResult.estimatedGg;
try {
final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
m1.add(mg);
final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
angularRateTrue.setElementAtIndex(0, angularRateTrueX);
angularRateTrue.setElementAtIndex(1, angularRateTrueY);
angularRateTrue.setElementAtIndex(2, angularRateTrueZ);
m1.multiply(angularRateTrue);
final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
fTrue.setElementAtIndex(0, fTrueX);
fTrue.setElementAtIndex(1, fTrueY);
fTrue.setElementAtIndex(2, fTrueZ);
final var m2 = gg.multiplyAndReturnNew(fTrue);
m1.add(m2);
final var angularRateMeasX2 = biasX + m1.getElementAtIndex(0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3426 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3510 |
final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must be 3x1
* and is expressed in meters per squared
* second (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4871 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7015 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5004 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7159 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5141 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8434 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5908 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8143 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6041 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8287 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6178 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7306 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6572 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7817 |
final double timeInterval, final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6690 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7700 |
final double[] bias, final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
qualityScores, position, turntableRotationRate, timeInterval, measurements, bias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 7 samples.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5399 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7497 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5525 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7638 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5659 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8892 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6416 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8605 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6542 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8746 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6676 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7782 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7067 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8289 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7181 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8173 |
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(qualityScores, position, turntableRotationRate,
timeInterval, measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size, if either
* turntable rotation rate or
* time interval is zero or negative or
* if provided quality scores length is
* smaller than 10 samples.
*/
public static RobustTurntableGyroscopeCalibrator create(
final double[] qualityScores, final ECEFPosition position, final double turntableRotationRate, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/generators/AccelerometerAndGyroscopeMeasurementsGenerator.java | 253 |
| com/irurueta/navigation/inertial/calibration/generators/AccelerometerGyroscopeAndMagnetometerMeasurementsGenerator.java | 331 |
gyroscopeMeasurementsGenerator.setTimeInterval(timeInterval);
}
/**
* Gets time interval between input samples.
*
* @return time interval between input samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(getTimeInterval(), TimeUnit.SECOND);
}
/**
* Gets time interval between input samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(getTimeInterval());
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between input samples.
*
* @param timeInterval time interval between input samples.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
TimeUnit.SECOND));
}
/**
* Gets minimum number of samples required in a static interval to be taken into account.
* Smaller static intervals will be discarded.
*
* @return minimum number of samples required in a static interval to be taken into account.
*/
public int getMinStaticSamples() {
return accelerometerMeasurementsGenerator.getMinStaticSamples();
}
/**
* Sets minimum number of samples required in a static interval to be taken into account.
* Smaller static intervals will be discarded.
*
* @param minStaticSamples minimum number of samples required in a static interval to be
* taken into account.
* @throws LockedException if generator is busy.
* @throws IllegalArgumentException if provided value is less than 2.
*/
public void setMinStaticSamples(final int minStaticSamples) throws LockedException {
if (running) {
throw new LockedException();
}
accelerometerMeasurementsGenerator.setMinStaticSamples(minStaticSamples);
gyroscopeMeasurementsGenerator.setMinStaticSamples(minStaticSamples); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 596 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1641 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 580 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1673 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured angular rates and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates gyroscope calibration parameters containing scale factors,
* cross-coupling errors and G-dependent coupling.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5002 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2977 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 480 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4157 |
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5823 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5929 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2959 |
fmeasZ = point[2];
gradientEstimator.gradient(params, derivatives);
return evaluateGeneral(params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var m11 = result[3];
final var m21 = result[4];
final var m31 = result[5];
final var m12 = result[6];
final var m22 = result[7];
final var m32 = result[8];
final var m13 = result[9];
final var m23 = result[10];
final var m33 = result[11];
final var bias = new Matrix(BodyKinematics.COMPONENTS, 1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1081 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2688 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2910 |
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether estimator is ready to start the estimator.
*
* @return true if estimator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether estimator is currently running or not.
*
* @return true if estimator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if estimator is currently running.
* @throws NotReadyException if estimator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 468 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2865 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3225 |
} catch (final AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 1024 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 1101 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1250 |
b.setElementAtIndex(i, fMeasZ - fTrueZ);
i++;
}
final var unknowns = Utils.solve(a, b);
final var bx = unknowns.getElementAtIndex(0);
final var by = unknowns.getElementAtIndex(1);
final var bz = unknowns.getElementAtIndex(2);
final var sx = unknowns.getElementAtIndex(3);
final var sy = unknowns.getElementAtIndex(4);
final var sz = unknowns.getElementAtIndex(5);
final var mxy = unknowns.getElementAtIndex(6);
final var mxz = unknowns.getElementAtIndex(7);
final var myx = unknowns.getElementAtIndex(8);
final var myz = unknowns.getElementAtIndex(9);
final var mzx = unknowns.getElementAtIndex(10);
final var mzy = unknowns.getElementAtIndex(11); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4302 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4941 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final Matrix initialMa, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided gravity norm value is negative or
* if provided quality scores length is smaller
* than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4145 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4747 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, initialMa, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements list of body kinematics measurements taken at a given position with
* different unknown orientations and containing the standard deviations
* of accelerometer and gyroscope measurements.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 10 samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4828 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5465 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final Matrix initialBias, final Matrix initialMa,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(qualityScores,
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(qualityScores,
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided gravity norm value is negative or
* if provided quality scores length is smaller
* than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4659 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5248 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final Matrix initialBias, final Matrix initialMa,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, initialMa, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements list of body kinematics measurements taken at a given position with
* different unknown orientations and containing the standard deviations
* of accelerometer and gyroscope measurements.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 13 samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1085 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3158 |
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurementsOrSequences() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the calibration.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3025 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 413 |
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurementsOrSequences() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the calibration.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors,
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3604 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4267 |
for (int j = 6, i = 9; j < 15; j++, i++) {
jacobian.setElementAt(i, j, 1.0);
}
// propagated covariance is J * Cov * J'
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Internal method to perform general calibration.
*
* @throws AlgebraException if there are numerical errors.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateGeneral() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException {
// The gyroscope model is:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
// Hence:
// [Ωmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [0 1 0] [myx sy myz] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [0 0 1] [mzx mzy sz ] [Ωtruez] [g31 g32 g33][ftruez]
// [Ωmeasx] = [bx] + ( [1+sx mxy mxz ]) [Ωtruex] + [g11 g12 g13][ftruex]
// [Ωmeasy] [by] [myx 1+sy myz ] [Ωtruey] [g21 g22 g23][ftruey]
// [Ωmeasz] [bz] [mzx mzy 1+sz] [Ωtruez] [g31 g32 g33][ftruez]
// Ωmeasx = bx + (1+sx) * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + myx * Ωtruex + (1+sy) * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + (1+sz) * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy, g11, g12, g13, g21, g22, g23,
// g31, g32, g33
// Reordering:
// Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + myx * Ωtruex + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + mzx * Ωtruex + mzy * Ωtruey + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Ωmeasx - Ωtruex - bx = sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy - Ωtruey - by = myx * Ωtruex + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz - Ωtruez - bz = mzx * Ωtruex + mzy * Ωtruey + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// [Ωtruex 0 0 Ωtruey Ωtruez 0 0 0 0 ftruex ftruey ftruez 0 0 0 0 0 0 ][sx ] = [Ωmeasx - Ωtruex - bx]
// [0 Ωtruey 0 0 0 Ωtruex Ωtruez 0 0 0 0 0 ftruex ftruey ftruez 0 0 0 ][sy ] [Ωmeasy - Ωtruey - by]
// [0 0 Ωtruez 0 0 0 0 Ωtruex Ωtruey 0 0 0 0 0 0 ftruex ftruey ftruez][sz ] [Ωmeasz - Ωtruez - bz]
// [mxy]
// [mxz]
// [myx]
// [myz]
// [mzx]
// [mzy]
// [g11]
// [g12]
// [g13]
// [g21]
// [g22]
// [g23]
// [g31]
// [g32]
// [g33]
fitter.setFunctionEvaluator(new LevenbergMarquardtMultiVariateFunctionEvaluator() {
@Override
public int getNumberOfDimensions() {
// Input points are true angular rate + specific force coordinates
return 2 * BodyKinematics.COMPONENTS;
}
@Override
public int getNumberOfVariables() {
// The multivariate function returns the components of measured angular rate
return BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[GENERAL_UNKNOWNS];
initial[0] = initialSx; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3760 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3829 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
listener);
default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4043 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4130 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
default -> new PROMedSRobustEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3553 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3640 |
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param commonAxisUsed indicates whether z-axis is
* assumed to be common for
* accelerometer and gyroscope.
* @param estimateGDependentCrossBiases true if G-dependent cross biases
* will be estimated, false
* otherwise.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must be 3x1 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4871 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8143 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5004 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8287 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5908 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7015 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6041 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7159 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10441 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 10940 |
final var mg = preliminaryResult.estimatedMg;
final var gg = preliminaryResult.estimatedGg;
final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
m1.add(mg);
final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
angularRateTrue.setElementAtIndex(0, angularRateTrueX);
angularRateTrue.setElementAtIndex(1, angularRateTrueY);
angularRateTrue.setElementAtIndex(2, angularRateTrueZ);
m1.multiply(angularRateTrue);
final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
fTrue.setElementAtIndex(0, fTrueX);
fTrue.setElementAtIndex(1, fTrueY);
fTrue.setElementAtIndex(2, fTrueZ);
final var m2 = gg.multiplyAndReturnNew(fTrue);
m1.add(m2);
final var angularRateMeasX2 = biasX + m1.getElementAtIndex(0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5399 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8605 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5525 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8746 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6416 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7497 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6542 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7638 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4842 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5513 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final Matrix hardIron, final Matrix initialMm,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, initialMm, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, initialMm, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 8030 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 6995 |
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationBodyMagneticFluxDensity>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierMeasurements.add(measurements.get(i));
}
}
try {
innerCalibrator.setInitialHardIron(preliminaryResult.estimatedHardIron);
innerCalibrator.setInitialMm(preliminaryResult.estimatedMm);
innerCalibrator.setCommonAxisUsed(commonAxisUsed);
innerCalibrator.setGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5330 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5997 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final Matrix initialHardIron, final Matrix initialMm,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1345 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 984 |
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var fMeasX = measuredKinematics.getFx();
final var fMeasY = measuredKinematics.getFy();
final var fMeasZ = measuredKinematics.getFz();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, fTrueX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1455 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 876 |
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var fMeasX = measuredKinematics.getFx();
final var fMeasY = measuredKinematics.getFy();
final var fMeasZ = measuredKinematics.getFz();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
a.setElementAt(i, 0, fTrueX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 794 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 164 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 801 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java | 576 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 687 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java | 561 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 170 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 167 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 882 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 879 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 884 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1576 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1285 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1255 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 411 |
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4280 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5100 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3790 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4620 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 405 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1590 |
}
/**
* Gets a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @return a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
*/
@Override
public Collection<FrameBodyMagneticFluxDensity> getMeasurements() {
return measurements;
}
/**
* Sets a collection of body magnetic flux density measurements taken at different
* frames (positions, orientations and velocities).
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate the a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*
* @param measurements collection of body magnetic flux density measurements
* taken at different frames (positions, orientations
* and velocities).
* @throws LockedException if estimator is currently running.
*/
@Override
public void setMeasurements(final Collection<? extends FrameBodyMagneticFluxDensity> measurements)
throws LockedException {
if (running) {
throw new LockedException();
}
//noinspection unchecked
this.measurements = (Collection<FrameBodyMagneticFluxDensity>) measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public MagnetometerCalibratorMeasurementType getMeasurementType() {
return MagnetometerCalibratorMeasurementType.FRAME_BODY_MAGNETIC_FLUX_DENSITY;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return false;
}
/**
* Indicates whether this calibrator requires quality scores for each
* measurement or not.
*
* @return true if quality scores are required, false otherwise.
*/
@Override
public boolean isQualityScoresRequired() {
return false;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer.
* When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
*
* @return true if z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer, false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mm matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for
* accelerometer, gyroscope and magnetometer, false
* otherwise.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this calibrator.
*
* @return listener to handle events raised by this calibrator.
*/
@Override
public KnownFrameMagnetometerLinearLeastSquaresCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/noise/WindowedBodyKinematicsNoiseEstimator.java | 225 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedMeasurementNoiseEstimator.java | 182 |
reset();
}
/**
* Gets time interval between body kinematics samples expressed in
* seconds (s).
*
* @return time interval between accelerometer triad samples.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between body kinematics samples expressed in
* seconds (s).
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval < 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between body kinematics samples.
*
* @return time interval between accelerometer triad samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between body kinematics samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between body kinematics samples.
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(),
TimeUnit.SECOND));
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public WindowedBodyKinematicsNoiseEstimatorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3978 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4378 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4726 |
return initial;
}
@Override
public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
final Matrix jacobian) {
// We know that:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez
// Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
// sz, mxy, mxz, myx, myz, mzx and mzy is:
// d(fmeasx)/d(bx) = 1.0
// d(fmeasx)/d(by) = 0.0
// d(fmeasx)/d(bz) = 0.0
// d(fmeasx)/d(sx) = ftruex
// d(fmeasx)/d(sy) = 0.0
// d(fmeasx)/d(sz) = 0.0
// d(fmeasx)/d(mxy) = ftruey
// d(fmeasx)/d(mxz) = ftruez
// d(fmeasx)/d(myx) = 0.0
// d(fmeasx)/d(myz) = 0.0
// d(fmeasx)/d(mzx) = 0.0
// d(fmeasx)/d(mzy) = 0.0
// d(fmeasy)/d(bx) = 0.0
// d(fmeasy)/d(by) = 1.0
// d(fmeasy)/d(bz) = 0.0
// d(fmeasy)/d(sx) = 0.0
// d(fmeasy)/d(sy) = ftruey
// d(fmeasy)/d(sz) = 0.0
// d(fmeasy)/d(mxy) = 0.0
// d(fmeasy)/d(mxz) = 0.0
// d(fmeasy)/d(myx) = ftruex
// d(fmeasy)/d(myz) = ftruez
// d(fmeasy)/d(mzx) = 0.0
// d(fmeasy)/d(mzy) = 0.0
// d(fmeasz)/d(bx) = 0.0
// d(fmeasz)/d(by) = 0.0
// d(fmeasz)/d(bz) = 1.0
// d(fmeasz)/d(sx) = 0.0
// d(fmeasz)/d(sy) = 0.0
// d(fmeasz)/d(sz) = ftruez
// d(fmeasz)/d(mxy) = 0.0
// d(fmeasz)/d(mxz) = 0.0
// d(fmeasz)/d(myx) = 0.0
// d(fmeasz)/d(myz) = 0.0
// d(fmeasz)/d(mzx) = ftruex
// d(fmeasz)/d(mzy) = ftruey
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var sx = params[3];
final var sy = params[4];
final var sz = params[5];
final var mxy = params[6];
final var mxz = params[7];
final var myx = params[8];
final var myz = params[9];
final var mzx = params[10];
final var mzy = params[11];
final var ftruex = point[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1676 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1594 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1576 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1695 |
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples
* (10 or 13).
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4638 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4735 |
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4154 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4253 |
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
qualityScores, sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
qualityScores, sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1944 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1943 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1948 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2039 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 469 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2239 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 497 |
this(measurements, commonAxisUsed, initialBias, initialMa);
this.listener = listener;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasXAsAcceleration() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3131 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3673 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Acceleration groundTruthGravityNorm, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3039 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3549 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final NEDPosition position, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3659 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4200 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Acceleration groundTruthGravityNorm, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3564 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4068 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final NEDPosition position, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java | 1219 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3399 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 1189 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3464 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1215 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3396 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sequence.
* The larger the score value the better the quality of the sequence.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sequence.
* The larger the score value the better the quality of the sequence.
*
* @param qualityScores quality scores corresponding to each sequence.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1185 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3459 |
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sequence.
* The larger the score value the better the quality of the sequence.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sequence.
* The larger the score value the better the quality of the sequence.
*
* @param qualityScores quality scores corresponding to each sequence.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether calibrator is ready to find a solution.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3419 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4004 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron, final Matrix initialMm,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final ECEFPosition position, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3927 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4506 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix initialHardIron, final Matrix initialMm,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final ECEFPosition position, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9356 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9401 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, convertDistanceToDouble(oldX),
convertDistanceToDouble(oldY), convertDistanceToDouble(oldZ), oldC,
oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
convertAngularSpeedToDouble(angularRateX), convertAngularSpeedToDouble(angularRateY),
convertAngularSpeedToDouble(angularRateZ), result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/BodyKinematicsGenerator.java | 117 |
| com/irurueta/navigation/inertial/calibration/BodyKinematicsGenerator.java | 310 |
final IMUErrors errors, final Random random, final Collection<BodyKinematics> result) {
try {
final var trueFibb = new Matrix(BodyKinematics.COMPONENTS, 1);
final var ma = errors.getAccelerometerScaleFactorAndCrossCouplingErrors();
final var ba = errors.getAccelerometerBiasesAsMatrix();
final var trueOmegaIbb = new Matrix(BodyKinematics.COMPONENTS, 1);
final var mg = errors.getGyroScaleFactorAndCrossCouplingErrors();
final var bg = errors.getGyroBiasesAsMatrix();
final var gg = errors.getGyroGDependentBiases();
final var identity = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
final var tmp33 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
final var tmp31a = new Matrix(BodyKinematics.COMPONENTS, 1);
final var tmp31b = new Matrix(BodyKinematics.COMPONENTS, 1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 1823 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 717 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2657 |
this.listener = listener;
}
/**
* Gets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return known x coordinate of accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasX known x coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return known y coordinate of accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasY known y coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return known z coordinate of accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasZ known z coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x coordinate of accelerometer bias.
*
* @return known x coordinate of accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1509 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1676 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1576 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1695 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1594 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1517 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3399 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 344 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3464 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 344 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 346 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1944 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1955 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1943 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1948 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3101 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4934 |
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
// g-dependent cross biases G
final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
for (int i = 0, j = k; i < num; i++, j++) {
initial[j] = initG.getElementAtIndex(i);
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4237 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5642 |
private double evaluateCommonAxisWithGDependentCrossBiases(final int i, final double[] params)
throws EvaluationException {
final var m11 = params[0];
final var m12 = params[1];
final var m22 = params[2];
final var m13 = params[3];
final var m23 = params[4];
final var m33 = params[5];
final var g11 = params[6];
final var g21 = params[7];
final var g31 = params[8];
final var g12 = params[9];
final var g22 = params[10];
final var g32 = params[11];
final var g13 = params[12];
final var g23 = params[13];
final var g33 = params[14];
return evaluate(i, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33, g11, g21, g31, g12, g22, g32, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4683 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6811 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5719 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7939 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5220 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7301 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6237 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8409 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedMeasurementNoiseEstimator.java | 109 |
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java | 144 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedBodyKinematicsNoiseEstimator.java | 226 |
}
/**
* Gets time interval between accelerometer triad samples expressed in
* seconds (s).
*
* @return time interval between accelerometer triad samples.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between accelerometer triad samples expressed in
* seconds (s).
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval < 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between accelerometer triad samples.
*
* @return time interval between accelerometer triad samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between accelerometer triad samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between accelerometer triad samples.
*
* @param timeInterval time interval between accelerometer triad samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(),
timeInterval.getUnit(), TimeUnit.SECOND));
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public L getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1791 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1694 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1810 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1712 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1632 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3514 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 459 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3579 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 459 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2060 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2057 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2062 |
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1808 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1712 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 474 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1828 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1734 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1649 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3534 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 478 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3597 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 477 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2079 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2077 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2081 |
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3982 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4630 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3840 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4442 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
* quality scores array is smaller than 13
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4510 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5157 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4353 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4945 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
* quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4393 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5225 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3903 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4745 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4513 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5184 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5001 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5667 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/FrameBodyKinematics.java | 352 |
| com/irurueta/navigation/inertial/calibration/FrameBodyMagneticFluxDensity.java | 353 |
}
/**
* Gets current body position (which will typically remain constant),
* velocity (which will typically be zero) and orientation (which usually changes
* with each measurement to perform calibration of a single device) resolved
* around ECEF axes associated to body kinematics measurement.
*
* @return current ECEF frame associated to body kinematics measurement or null if
* not available.
*/
public ECEFFrame getFrame() {
return frame;
}
/**
* Sets current body position (which will typically remain constant),
* velocity (which will typically be zero) and orientation (which usually changes
* with each measurement to perform calibration of a single device) resolved
* around ECEF axes associated to body kinematics measurement.
*
* @param frame current ECEF frame
*/
public void setFrame(final ECEFFrame frame) {
this.frame = frame;
}
/**
* Gets current body position (which will typically remain constant),
* velocity (which will typically be zero) and orientation (which usually changes
* with each measurement to perform calibration of a single device) resolved
* around NED axes associated to body kinematics measurement.
*
* @return current NED frame associated to body kinematics measurement or null if
* not available.
*/
public NEDFrame getNedFrame() {
return frame != null ? ECEFtoNEDFrameConverter.convertECEFtoNEDAndReturnNew(frame) : null;
}
/**
* Gets current body position (which will typically remain constant),
* velocity (which will typically be zero) and orientation (which usually changes
* with each measurement to perform calibration of a single device) resolved
* around NED axes associated to body kinematics measurement.
*
* @param result instance where result data will be stored if available.
* @return true if result instance was updated, false otherwise.
*/
public boolean getNedFrame(final NEDFrame result) {
if (frame != null) {
ECEFtoNEDFrameConverter.convertECEFtoNED(frame, result);
return true;
} else {
return false;
}
}
/**
* Sets current body position (which will typically remain constant),
* velocity (which will typically be zero) and orientation (which usually changes
* with each measurement to perform calibration of a single device) resolved
* around NED axes associated to body kinematics measurement.
* <p>
* This method will internally store the corresponding ECEF frame to provided
* NED frame value.
*
* @param nedFrame current NED frame associated to body kinematics measurement
* to be set.
*/
public void setNedFrame(final NEDFrame nedFrame) {
if (nedFrame != null) {
if (frame != null) {
NEDtoECEFFrameConverter.convertNEDtoECEF(nedFrame, frame);
} else {
frame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
}
} else {
frame = null;
}
}
/**
* Gets previous body position (which will typically remain constant),
* velocity (which will typically be zero) and orientation (which usually changes
* with each measurement to perform calibration of a single device) resolved
* around ECEF axes associated to previous body kinematics measurement.
*
* @return previous ECEF frame associated to previous body kinematics measurement or
* null if not available.
*/
public ECEFFrame getPreviousFrame() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4355 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1026 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2423 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1314 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1736 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1030 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2970 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2417 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3499 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1333 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3461 |
}
/**
* Gets known bias as a column matrix.
*
* @return known bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known bias as an array.
*
* @param bias bias to be set.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException {
if (running) {
throw new LockedException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1089 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 409 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3033 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3166 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether estimator is ready to start the estimator.
*
* @return true if estimator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether estimator is currently running or not.
*
* @return true if estimator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if estimator is currently running.
* @throws NotReadyException if estimator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2696 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2918 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1093 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 421 |
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or not.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3789 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4075 |
final var mzy = params[8];
final var g11 = params[9];
final var g21 = params[10];
final var g31 = params[11];
final var g12 = params[12];
final var g22 = params[13];
final var g32 = params[14];
final var g13 = params[15];
final var g23 = params[16];
final var g33 = params[17];
final var omegatruex = point[0];
final var omegatruey = point[1];
final var omegatruez = point[2];
final var ftruex = point[3];
final var ftruey = point[4];
final var ftruez = point[5];
result[0] = biasX + omegatruex + sx * omegatruex + mxy * omegatruey + mxz * omegatruez | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1614 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1634 |
public MSACRobustKnownBiasTurntableGyroscopeCalibrator(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener) {
super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1646 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1666 |
public MSACRobustTurntableGyroscopeCalibrator(
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener) {
super(position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4683 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7939 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5072 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7232 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5719 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6811 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6109 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8360 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5220 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8409 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5594 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7710 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6237 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7301 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6611 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8820 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 3978 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2046 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1229 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1338 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1294 |
this.biasZ = convertAcceleration(biasZ);
}
/**
* Gets known accelerometer bias.
*
* @return known accelerometer bias.
*/
@Override
public AccelerationTriad getBiasAsTriad() {
return new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known accelerometer bias.
*
* @param result instance where result will be stored.
*/
@Override
public void getBiasAsTriad(final AccelerationTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Sets known accelerometer bias.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBias(final AccelerationTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAcceleration(bias.getValueX(), bias.getUnit());
biasY = convertAcceleration(bias.getValueY(), bias.getUnit());
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor.
*
* @return initial x scaling factor.
*/
@Override
public double getInitialSx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5795 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4518 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5901 |
return BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[GENERAL_UNKNOWNS];
// biases b
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
initial[i] = initialB.getElementAtIndex(i);
}
// cross coupling errors M
final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
for (int i = 0, j = BodyKinematics.COMPONENTS; i < num; i++, j++) {
initial[j] = initialM.getElementAtIndex(i);
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1642 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1664 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 790 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 799 |
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
gravityNorm = computeGravityNorm();
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 704 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 814 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 759 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 185 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 823 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 768 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1641 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 190 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1673 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 187 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 902 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 909 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 900 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 905 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode") | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3984 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4139 |
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4138 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4783 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4218 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4861 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMa,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, initialMa, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4260 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4900 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final Matrix initialMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4632 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4784 |
final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is smaller
* than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Acceleration groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3841 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3988 |
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
* quality scores array is smaller than 13
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3987 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4589 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3
* or if quality scores array is smaller than 10
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4063 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4665 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMa,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, initialMa, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4104 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4706 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final Matrix initialMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, initialMa);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4443 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4590 |
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
* quality scores array is smaller than 10
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4512 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4667 |
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4665 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5308 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4745 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5386 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMa,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4786 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5424 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final Matrix initialBias, final Matrix initialMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5159 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5310 |
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is smaller
* than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Acceleration groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4355 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4504 |
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
* quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4502 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5091 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final Matrix initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3
* or if quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4578 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5167 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMa,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, initialMa, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4618 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5207 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final Matrix initialBias, final Matrix initialMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, initialMa);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4947 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5093 |
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
* quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3695 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4009 |
m.setElementAtIndex(5, 0.0);
m.setElementAtIndex(6, m13);
m.setElementAtIndex(7, m23);
m.setElementAtIndex(8, m33);
final var g = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
g.setElementAtIndex(0, g11);
g.setElementAtIndex(1, g21);
g.setElementAtIndex(2, g31);
g.setElementAtIndex(3, g12);
g.setElementAtIndex(4, g22);
g.setElementAtIndex(5, g32);
g.setElementAtIndex(6, g13);
g.setElementAtIndex(7, g23);
g.setElementAtIndex(8, g33);
setResult(m, b, g);
// at this point covariance is expressed in terms of b, M and G, and must
// be expressed in terms of bg, Mg and Gg.
// We know that:
// bg = M * b
// Mg = M - I
// Gg = M * G
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [0 m22 m23]
// [0 0 m33]
// G = [g11 g12 g13]
// [g21 g22 g23]
// [g31 g32 g33]
// bg = [m11 m12 m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
// [0 m22 m23][by] [ m22 * by + m23 * bz] [bgy]
// [0 0 m33][bz] [ m33 * bz] [bgz]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [0 m22 - 1 m23 ]
// [mzx mzy sz ] [0 0 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = [m11 m12 m13][g11 g12 g13]
// [gg21 gg22 gg23] [0 m22 m23][g21 g22 g23]
// [gg31 gg32 gg33] [0 0 m33][g31 g32 g33]
// Defining the linear application:
// F(b, M, G) = F(bx, by, bz, m11, m12, m22, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
// as:
// [bgx] = [m11 * bx + m12 * by + m13 * bz]
// [bgy] [ m22 * by + m23 * bz]
// [bgz] [ m33 * bz]
// [sx] [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [0]
// [myz] [m23]
// [mzx] [0]
// [mzy] [0]
// [gg11] [m11 * g11 + m12 * g21 + m13 * g31]
// [gg21] [ m22 * g21 + m23 * g31]
// [gg31] [ m33 * g31]
// [gg12] [m11 * g12 + m12 * g22 + m13 * g32]
// [gg22] [ m22 * g22 + m23 * g32]
// [gg32] [ m33 * g32]
// [gg13] [m11 * g13 + m12 * g23 + m13 * g33]
// [gg23] [ m22 * g23 + m23 * g33]
// [gg33] [ m33 * g33]
// Then the Jacobian of F(b, M, G) is:
// J = [m11 m12 m13 bx by 0 bz 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 m22 m23 0 0 by 0 bz 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 m33 0 0 0 0 0 bz 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 g11 g21 0 g31 0 0 m11 m12 m13 0 0 0 0 0 0 ]
// [0 0 0 0 0 g21 0 g31 0 0 m22 m23 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 g31 0 0 m33 0 0 0 0 0 0 ]
// [0 0 0 g12 g22 0 g32 0 0 0 0 0 m11 m12 m13 0 0 0 ]
// [0 0 0 0 0 g22 0 g32 0 0 0 0 0 m22 m23 0 0 0 ]
// [0 0 0 0 0 0 0 0 g32 0 0 0 0 0 m33 0 0 0 ]
// [0 0 0 g13 g23 0 g33 0 0 0 0 0 0 0 0 m11 m12 m13]
// [0 0 0 0 0 g23 0 g33 0 0 0 0 0 0 0 0 m22 m23]
// [0 0 0 0 0 0 0 0 g33 0 0 0 0 0 0 0 0 m33]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5882 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 11015 |
innerCalibrator.setSequences(seqs);
innerCalibrator.calibrate();
innerCalibrator.getEstimatedBiases(result.estimatedBiases);
result.estimatedMg = innerCalibrator.getEstimatedMg();
result.estimatedGg = innerCalibrator.getEstimatedGg();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5014 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5359 |
mm.setElementAtIndex(5, 0.0);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33);
final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mg.setElementAtIndex(0, g11);
mg.setElementAtIndex(1, g21);
mg.setElementAtIndex(2, g31);
mg.setElementAtIndex(3, g12);
mg.setElementAtIndex(4, g22);
mg.setElementAtIndex(5, g32);
mg.setElementAtIndex(6, g13);
mg.setElementAtIndex(7, g23);
mg.setElementAtIndex(8, g33);
setResult(mm, mb, mg);
// at this point covariance is expressed in terms of b, M and G, and must
// be expressed in terms of bg, Mg and Gg.
// We know that:
// bg = M * b
// Mg = M - I
// Gg = M * G
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [0 m22 m23]
// [0 0 m33]
// G = [g11 g12 g13]
// [g21 g22 g23]
// [g31 g32 g33]
// bg = [m11 m12 m13][bx] = [m11 * bx + m12 * by + m13 * bz] = [bgx]
// [0 m22 m23][by] [ m22 * by + m23 * bz] [bgy]
// [0 0 m33][bz] [ m33 * bz] [bgz]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [0 m22 - 1 m23 ]
// [mzx mzy sz ] [0 0 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = [m11 m12 m13][g11 g12 g13]
// [gg21 gg22 gg23] [0 m22 m23][g21 g22 g23]
// [gg31 gg32 gg33] [0 0 m33][g31 g32 g33]
// Defining the linear application:
// F(b, M, G) = F(bx, by, bz, m11, m12, m22, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
// as:
// [bgx] = [m11 * bx + m12 * by + m13 * bz]
// [bgy] [ m22 * by + m23 * bz]
// [bgz] [ m33 * bz]
// [sx] [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [0]
// [myz] [m23]
// [mzx] [0]
// [mzy] [0]
// [gg11] [m11 * g11 + m12 * g21 + m13 * g31]
// [gg21] [ m22 * g21 + m23 * g31]
// [gg31] [ m33 * g31]
// [gg12] [m11 * g12 + m12 * g22 + m13 * g32]
// [gg22] [ m22 * g22 + m23 * g32]
// [gg32] [ m33 * g32]
// [gg13] [m11 * g13 + m12 * g23 + m13 * g33]
// [gg23] [ m22 * g23 + m23 * g33]
// [gg33] [ m33 * g33]
// Then the Jacobian of F(b, M, G) is:
// J = [m11 m12 m13 bx by 0 bz 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 m22 m23 0 0 by 0 bz 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 m33 0 0 0 0 0 bz 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 g11 g21 0 g31 0 0 m11 m12 m13 0 0 0 0 0 0 ]
// [0 0 0 0 0 g21 0 g31 0 0 m22 m23 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 g31 0 0 m33 0 0 0 0 0 0 ]
// [0 0 0 g12 g22 0 g32 0 0 0 0 0 m11 m12 m13 0 0 0 ]
// [0 0 0 0 0 g22 0 g32 0 0 0 0 0 m22 m23 0 0 0 ]
// [0 0 0 0 0 0 0 0 g32 0 0 0 0 0 m33 0 0 0 ]
// [0 0 0 g13 g23 0 g33 0 0 0 0 0 0 0 0 m11 m12 m13]
// [0 0 0 0 0 g23 0 g33 0 0 0 0 0 0 0 0 m22 m23]
// [0 0 0 0 0 0 0 0 g33 0 0 0 0 0 0 0 0 m33]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 5138 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 5301 |
final double[] hardIron,
final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron,
listener);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron,
listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron matrix is not 3x1,
* or if provided quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4515 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4672 |
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4670 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5341 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final Matrix hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4754 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5425 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
final Matrix initialMm, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, initialMm, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, initialMm, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4798 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5469 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final Matrix hardIron, final Matrix initialMm, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, initialMm);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, initialMm);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5186 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5343 |
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 5651 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 5814 |
final double[] initialHardIron,
final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron,
listener);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron,
listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron matrix is not 3x1,
* or if provided quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5003 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5160 |
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5158 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5825 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final Matrix initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5242 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5909 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
final Matrix initialMm, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, initialMm, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, initialMm, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5286 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5953 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final Matrix initialHardIron, final Matrix initialMm, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, initialMm);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, initialMm);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5669 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5827 |
final boolean commonAxisUsed, final double[] initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6361 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5797 |
fmeas.setElementAtIndex(0, fmeasX);
fmeas.setElementAtIndex(1, fmeasY);
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, bx); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1005 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1117 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1073 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 1819 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 2900 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1031 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 1823 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 2864 |
internalSetBias(bias);
}
/**
* Gets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return x coordinate of accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets known x coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasX x coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return y coordinate of accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets known y coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasY y coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @return z coordinate of accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets known z coordinate of accelerometer bias expressed in meters per squared
* second (m/s^2).
*
* @param biasZ z coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets known x coordinate of accelerometer bias.
*
* @return x coordinate of accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3755 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 4021 |
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
biasX, biasY, biasZ, commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
biasX, biasY, biasZ, commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param biasX known x coordinate of accelerometer bias.
* @param biasY known y coordinate of accelerometer bias.
* @param biasZ known z coordinate of accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final double[] qualityScores, final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java | 1412 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 1382 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1430 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1401 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 766 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 752 |
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3812 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 4074 |
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param biasX known x coordinate of gyroscope bias.
* @param biasY known y coordinate of gyroscope bias.
* @param biasZ known z coordinate of gyroscope bias.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 6 samples.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final double[] qualityScores, final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4472 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5611 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4575 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5508 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param bias known gyroscope bias. This must have length
* 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustKnownBiasTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4808 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6947 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4937 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7087 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5072 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8360 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5845 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8075 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5974 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8215 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6109 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7232 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5005 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6127 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must be 3x1 and
* is expressed in radians per second
* (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5110 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6022 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
default -> new PROMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param position position where body kinematics measures
* have been taken.
* @param turntableRotationRate constant rotation rate at which the
* turntable is spinning. Must be
* expressed in radians per second (rad/s).
* @param timeInterval time interval between measurements being
* captured expressed in seconds (s).
* @param measurements collection of body kinematics
* measurements with standard deviations
* taken at the same position with zero
* velocity and unknown different
* orientations.
* @param initialBias initial gyroscope bias to be used to
* find a solution. This must have
* length 3 and is expressed in radians
* per second (rad/s).
* @param initialMg initial gyroscope scale factors and
* cross coupling errors matrix. Must
* be 3x3.
* @param initialGg initial gyroscope G-dependent cross
* biases introduced on the gyroscope by
* the specific forces sensed by the
* accelerometer. Must be 3x3.
* @param listener listener to handle events raised by
* this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if either
* turntable rotation rate or
* time interval is zero or negative.
*/
public static RobustTurntableGyroscopeCalibrator create(
final ECEFPosition position, final double turntableRotationRate, final double timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5339 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7432 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5461 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7567 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5594 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8820 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6356 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8540 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6478 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8675 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6611 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7710 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 285 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 284 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1001 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1002 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 537 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 535 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2146 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2142 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 555 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 555 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2160 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2159 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 359 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 358 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1078 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1077 |
setupWmmEstimator();
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.MSAC; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 8065 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 7031 |
innerCalibrator.setGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm);
innerCalibrator.setMeasurements(inlierMeasurements);
innerCalibrator.calibrate();
estimatedHardIron = innerCalibrator.getEstimatedHardIron();
estimatedMm = innerCalibrator.getEstimatedMm();
if (keepCovariance) {
estimatedCovariance = innerCalibrator.getEstimatedCovariance();
} else {
estimatedCovariance = null;
}
estimatedMse = innerCalibrator.getEstimatedMse();
estimatedChiSq = innerCalibrator.getEstimatedChiSq();
} catch (final LockedException | CalibrationException | NotReadyException e) {
estimatedCovariance = preliminaryResult.covariance;
estimatedHardIron = preliminaryResult.estimatedHardIron;
estimatedMm = preliminaryResult.estimatedMm;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
} else {
estimatedCovariance = preliminaryResult.covariance;
estimatedHardIron = preliminaryResult.estimatedHardIron;
estimatedMm = preliminaryResult.estimatedMm;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
}
/**
* Internally sets ground truth magnetic flux density norm to be expected at location where
* measurements have been made, expressed in Teslas (T).
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
* @throws IllegalArgumentException if provided value is negative.
*/
private void internalSetGroundTruthMagneticFluxDensityNorm(final Double groundTruthMagneticFluxDensityNorm) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 3758 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 709 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 1824 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1006 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1118 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1074 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 718 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2658 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 1820 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 2901 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1032 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 1824 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 2865 |
}
/**
* Gets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public double getBiasX() {
return biasX;
}
/**
* Sets x-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasX x-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasX(final double biasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasX = biasX;
}
/**
* Gets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return y-coordinate of known accelerometer bias.
*/
@Override
public double getBiasY() {
return biasY;
}
/**
* Sets y-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasY y-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasY(final double biasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasY = biasY;
}
/**
* Gets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @return z-coordinate of known accelerometer bias.
*/
@Override
public double getBiasZ() {
return biasZ;
}
/**
* Sets z-coordinate of known accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
*
* @param biasZ z-coordinate of known accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setBiasZ(final double biasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.biasZ = biasZ;
}
/**
* Gets x-coordinate of known accelerometer bias.
*
* @return x-coordinate of known accelerometer bias.
*/
@Override
public Acceleration getBiasXAsAcceleration() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4086 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1138 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1094 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2241 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 499 |
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasX() {
return initialBiasX;
}
/**
* Sets initial x-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasX initial x-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasX(final double initialBiasX) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasX = initialBiasX;
}
/**
* Gets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial y-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasY() {
return initialBiasY;
}
/**
* Sets initial y-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasY initial y-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasY(final double initialBiasY) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasY = initialBiasY;
}
/**
* Gets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @return initial z-coordinate of accelerometer bias.
*/
@Override
public double getInitialBiasZ() {
return initialBiasZ;
}
/**
* Sets initial z-coordinate of accelerometer bias to be used to find a solution.
* This is expressed in meters per squared second (m/s^2).
*
* @param initialBiasZ initial z-coordinate of accelerometer bias.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialBiasZ(final double initialBiasZ) throws LockedException {
if (running) {
throw new LockedException();
}
this.initialBiasZ = initialBiasZ;
}
/**
* Gets initial x-coordinate of accelerometer bias to be used to find a solution.
*
* @return initial x-coordinate of accelerometer bias.
*/
@Override
public Acceleration getInitialBiasXAsAcceleration() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6363 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6675 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3500 |
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, bx);
b.setElementAtIndex(1, by);
b.setElementAtIndex(2, bz);
invM.multiply(fmeas, ftrue); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 6409 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 6132 |
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
innerCalibrator.setMeasurements(inlierMeasurements);
innerCalibrator.calibrate();
estimatedBiases = innerCalibrator.getEstimatedBiases();
estimatedMa = innerCalibrator.getEstimatedMa();
estimatedMse = innerCalibrator.getEstimatedMse();
estimatedChiSq = innerCalibrator.getEstimatedChiSq();
if (keepCovariance) {
estimatedCovariance = innerCalibrator.getEstimatedCovariance();
} else {
estimatedCovariance = null;
}
} catch (final LockedException | CalibrationException | NotReadyException e) {
estimatedCovariance = preliminaryResult.covariance;
estimatedBiases = preliminaryResult.estimatedBiases;
estimatedMa = preliminaryResult.estimatedMa;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
} else {
estimatedCovariance = preliminaryResult.covariance;
estimatedBiases = preliminaryResult.estimatedBiases;
estimatedMa = preliminaryResult.estimatedMa;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3169 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3440 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4468 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4767 |
mm.setElementAtIndex(5, 0.0);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33);
final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mg.setElementAtIndex(0, g11);
mg.setElementAtIndex(1, g21);
mg.setElementAtIndex(2, g31);
mg.setElementAtIndex(3, g12);
mg.setElementAtIndex(4, g22);
mg.setElementAtIndex(5, g32);
mg.setElementAtIndex(6, g13);
mg.setElementAtIndex(7, g23);
mg.setElementAtIndex(8, g33);
setResult(mm, mg);
// at this point covariance is expressed in terms of M and G, and must
// be expressed in terms of Mg and Gg.
// We know that:
// Mg = M - I
// Gg = M * G
// M = [m11 m12 m13]
// [0 m22 m23]
// [0 0 m33]
// G = [g11 g12 g13]
// [g21 g22 g23]
// [g31 g32 g33]
// Mg = [sx mxy mxz] = [m11 - 1 m12 m13 ]
// [myx sy myz] [0 m22 - 1 m23 ]
// [mzx mzy sz ] [0 0 m33 - 1 ]
// Gg = [gg11 gg12 gg13] = [m11 m12 m13][g11 g12 g13]
// [gg21 gg22 gg23] [0 m22 m23][g21 g22 g23]
// [gg31 gg32 gg33] [0 0 m33][g31 g32 g33]
// Defining the linear application:
// F(M, G) = F(m11, m12, m22, m32=0, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33)
// as:
// [sx] = [m11 - 1]
// [sy] [m22 - 1]
// [sz] [m33 - 1]
// [mxy] [m12]
// [mxz] [m13]
// [myx] [0]
// [myz] [m23]
// [mzx] [0]
// [mzy] [0]
// [gg11] [m11 * g11 + m12 * g21 + m13 * g31]
// [gg21] [ m22 * g21 + m23 * g31]
// [gg31] [ m33 * g31]
// [gg12] [m11 * g12 + m12 * g22 + m13 * g32]
// [gg22] [ m22 * g22 + n23 * g32]
// [gg32] [ m33 * g32]
// [gg13] [m11 * g13 + m12 * g23 + m13 * g33]
// [gg23] [ m22 * g23 + m23 * g33]
// [gg33] [ m33 * g33]
// Then the Jacobian of F(M, G) is:
// J = [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 ]
// [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]
// [g11 g21 0 g31 0 0 m11 m12 m13 0 0 0 0 0 0 ]
// [0 0 g21 0 g31 0 0 m22 m23 0 0 0 0 0 0 ]
// [0 0 0 0 0 g31 0 0 m33 0 0 0 0 0 0 ]
// [g12 g22 0 g32 0 0 0 0 0 m11 m12 m13 0 0 0 ]
// [0 0 g22 0 g32 0 0 0 0 0 m22 m23 0 0 0 ]
// [0 0 0 0 0 g32 0 0 0 0 0 m33 0 0 0 ]
// [g13 g23 0 g33 0 0 0 0 0 0 0 0 m11 m12 m13]
// [0 0 g23 0 g33 0 0 0 0 0 0 0 0 m22 m23]
// [0 0 0 0 0 g33 0 0 0 0 0 0 0 0 m33]
// We know that the propagated covariance is J * Cov * J', hence:
final var jacobian = new Matrix(GENERAL_UNKNOWNS_AND_CROSS_BIASES, COMMON_Z_AXIS_UNKNOWNS_AND_CROSS_BIASES); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java | 693 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java | 680 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java | 1413 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 1383 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1431 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1402 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 767 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 753 |
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.MSAC; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 3015 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 10921 |
previousEcefFrame);
final var angularRateMeasX1 = measuredKinematics.getAngularRateX();
final var angularRateMeasY1 = measuredKinematics.getAngularRateY();
final var angularRateMeasZ1 = measuredKinematics.getAngularRateZ();
final var angularRateTrueX = expectedKinematics.getAngularRateX();
final var angularRateTrueY = expectedKinematics.getAngularRateY();
final var angularRateTrueZ = expectedKinematics.getAngularRateZ();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz();
final var b = preliminaryResult.estimatedBiases;
final var bx = b[0];
final var by = b[1];
final var bz = b[2];
final var mg = preliminaryResult.estimatedMg;
final var gg = preliminaryResult.estimatedGg; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanInitializer.java | 73 |
| com/irurueta/navigation/inertial/INSTightlyCoupledKalmanInitializer.java | 77 |
result.initialize(0.0);
for (var i = 0; i < 3; i++) {
result.setElementAt(i, i, initAttUnc2);
}
for (var i = 3; i < 6; i++) {
result.setElementAt(i, i, initVelUnc2);
}
for (var i = 6; i < 9; i++) {
result.setElementAt(i, i, initPosUnc2);
}
for (var i = 9; i < 12; i++) {
result.setElementAt(i, i, initBaUnc2);
}
for (var i = 12; i < 15; i++) {
result.setElementAt(i, i, initBgUnc2);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1696 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1865 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1768 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 530 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1885 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1787 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1718 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1883 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1787 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 549 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1903 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1809 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 871 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 983 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 930 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 354 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 992 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 939 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1707 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3589 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 534 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3654 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1724 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3609 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 553 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3672 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 877 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1810 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 359 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1842 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2138 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2135 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2157 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2153 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1074 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1070 |
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.PROMEDS; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4875 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4160 |
if (estimatedMg == null) {
estimatedMg = m;
} else {
estimatedMg.copyFrom(m);
}
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
estimatedMg.setElementAt(i, i, estimatedMg.getElementAt(i, i) - 1.0);
}
if (estimatedGg == null) {
estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedGg.initialize(0.0);
}
estimatedCovariance = fitter.getCovar();
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Computes gravity versor error at the end of a sequence using provided
* parameters.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param i row position.
* @param params array containing parameters for the general purpose case
* when G-dependent cross biases are taken into account. Must
* have length 18.
* @return error between estimated and measured gravity versor.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
throws EvaluationException {
final var bx = params[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4808 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8075 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4937 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8215 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5845 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6947 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5974 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7087 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5339 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8540 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5461 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8675 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6356 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7432 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6478 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7567 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronPositionAndInstantMagnetometerCalibrator.java | 924 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownPositionAndInstantMagnetometerCalibrator.java | 926 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2018 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2046 |
if (isRunning()) {
throw new LockedException();
}
this.position = position;
}
/**
* Gets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @return position where body magnetic flux density measurements have
* been taken or null if not available.
*/
public ECEFPosition getEcefPosition() {
if (position != null) {
final var result = new ECEFPosition();
getEcefPosition(result);
return result;
} else {
return null;
}
}
/**
* Gets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @param result instance where result will be stored.
* @return true if ECEF position could be computed, false otherwise.
*/
public boolean getEcefPosition(final ECEFPosition result) {
if (position != null) {
final var velocity = new ECEFVelocity();
NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(
position.getLatitude(), position.getLongitude(), position.getHeight(),
0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body magnetic flux density measurements have been
* taken expressed in ECEF coordinates.
*
* @param position position where body magnetic flux density have been
* taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final ECEFPosition position) throws LockedException {
if (isRunning()) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3811 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4558 |
jacobian.setElementAt(2, 5, ftruez);
jacobian.setElementAt(2, 6, 0.0);
jacobian.setElementAt(2, 7, 0.0);
jacobian.setElementAt(2, 8, 0.0);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var sx = result[3];
final var sy = result[4];
final var sz = result[5];
final var mxy = result[6];
final var mxz = result[7];
final var myz = result[8];
if (estimatedBiases == null) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 796 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 911 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 859 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 281 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 918 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java | 865 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1697 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1866 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1769 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 531 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1886 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1788 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1719 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1884 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1788 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 550 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1904 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1810 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 872 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 984 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 931 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 355 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 993 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 940 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 807 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1738 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 287 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1770 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1708 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3590 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 535 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3655 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1725 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3610 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 554 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3673 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 878 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1811 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 360 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1843 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1005 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 997 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2139 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2136 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2158 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2154 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1075 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1071 |
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.MSAC; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1509 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1676 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1576 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1695 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1594 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java | 1220 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1517 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 1190 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 344 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 344 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 346 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1944 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1955 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1943 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1948 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1507 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1571 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 336 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1690 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1593 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1216 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1511 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1186 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 340 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 339 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 342 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1941 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1948 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1940 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1944 |
this(measurements, bias, commonAxisUsed, listener);
internalSetQualityScores(qualityScores);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Returns quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @return quality scores corresponding to each sample.
*/
@Override
public double[] getQualityScores() {
return qualityScores;
}
/**
* Sets quality scores corresponding to each provided sample.
* The larger the score value the better the quality of the sample.
*
* @param qualityScores quality scores corresponding to each sample.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than minimum required samples.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setQualityScores(final double[] qualityScores) throws LockedException {
if (running) {
throw new LockedException();
}
internalSetQualityScores(qualityScores);
}
/**
* Indicates whether solver is ready to find a solution.
*
* @return true if solver is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3673 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3945 |
final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, commonAxisUsed,
listener);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, commonAxisUsed,
listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasY known y coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasZ known z coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3681 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4334 |
final var g33 = result[17];
final var b = new Matrix(BodyKinematics.COMPONENTS, 1);
b.setElementAtIndex(0, bx);
b.setElementAtIndex(1, by);
b.setElementAtIndex(2, bz);
final var m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
m.setElementAtIndex(0, m11);
m.setElementAtIndex(1, 0.0);
m.setElementAtIndex(2, 0.0);
m.setElementAtIndex(3, m12);
m.setElementAtIndex(4, m22);
m.setElementAtIndex(5, 0.0);
m.setElementAtIndex(6, m13);
m.setElementAtIndex(7, m23);
m.setElementAtIndex(8, m33); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3995 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4575 |
final var g33 = result[20];
final var b = new Matrix(BodyKinematics.COMPONENTS, 1);
b.setElementAtIndex(0, bx);
b.setElementAtIndex(1, by);
b.setElementAtIndex(2, bz);
final var m = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
m.setElementAtIndex(0, m11);
m.setElementAtIndex(1, m21);
m.setElementAtIndex(2, m31);
m.setElementAtIndex(3, m12);
m.setElementAtIndex(4, m22);
m.setElementAtIndex(5, m32);
m.setElementAtIndex(6, m13);
m.setElementAtIndex(7, m23);
m.setElementAtIndex(8, m33); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4223 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5037 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3730 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 4000 |
final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
final boolean commonAxisUsed, final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
biasX, biasY, biasZ, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
biasX, biasY, biasZ, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
biasX, biasY, biasZ, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, biasX, biasY, biasZ, commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, biasX, biasY, biasZ, commonAxisUsed, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 6 samples.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3733 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4557 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5000 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5697 |
final var g33 = result[17];
final var mb = new Matrix(BodyKinematics.COMPONENTS, 1);
mb.setElementAtIndex(0, bx);
mb.setElementAtIndex(1, by);
mb.setElementAtIndex(2, bz);
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mm.setElementAtIndex(0, m11);
mm.setElementAtIndex(1, 0.0);
mm.setElementAtIndex(2, 0.0);
mm.setElementAtIndex(3, m12);
mm.setElementAtIndex(4, m22);
mm.setElementAtIndex(5, 0.0);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5345 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5957 |
final var g33 = result[20];
final var mb = new Matrix(BodyKinematics.COMPONENTS, 1);
mb.setElementAtIndex(0, bx);
mb.setElementAtIndex(1, by);
mb.setElementAtIndex(2, bz);
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mm.setElementAtIndex(0, m11);
mm.setElementAtIndex(1, m21);
mm.setElementAtIndex(2, m31);
mm.setElementAtIndex(3, m12);
mm.setElementAtIndex(4, m22);
mm.setElementAtIndex(5, m32);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 818 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 826 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java | 602 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 712 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 585 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 194 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 191 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 906 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 904 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 908 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1577 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 406 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 735 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 845 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 216 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 854 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1286 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1581 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1256 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 410 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 628 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 739 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 612 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 221 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 409 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 412 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2011 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 218 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 220 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 933 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 940 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 931 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 936 |
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 64 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 70 |
public abstract class RobustKnownGravityNormAccelerometerCalibrator implements
AccelerometerNonLinearCalibrator, UnknownBiasNonLinearAccelerometerCalibrator, AccelerometerCalibrationSource,
AccelerometerBiasUncertaintySource, OrderedStandardDeviationBodyKinematicsAccelerometerCalibrator,
QualityScoredAccelerometerCalibrator {
/**
* Indicates whether by default a common z-axis is assumed for both the accelerometer
* and gyroscope.
*/
public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
/**
* Required minimum number of measurements when common z-axis is assumed.
*/
public static final int MINIMUM_MEASUREMENTS_COMMON_Z_AXIS =
BaseGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;
/**
* Required minimum number of measurements for the general case.
*/
public static final int MINIMUM_MEASUREMENTS_GENERAL =
BaseGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL;
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default.
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Contains a list of body kinematics measurements taken at a
* given position with different unknown orientations and containing the
* standard deviations of accelerometer and gyroscope measurements.
*/
protected List<StandardDeviationBodyKinematics> measurements;
/**
* Listener to be notified of events such as when calibration starts, ends or its
* progress significantly changes.
*/
protected RobustKnownGravityNormAccelerometerCalibratorListener listener; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3872 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3957 |
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3382 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3467 |
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a
* solution. This must have length 3 and is expressed
* in radians per second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param accelerometerBias known accelerometer bias. This must
* have length 3 and is expressed in
* meters per squared second
* (m/s^2).
* @param accelerometerMa known accelerometer scale factors and
* cross coupling matrix. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 811 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownMagneticFluxDensityNormMagnetometerCalibrator.java | 854 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Sets ground truth magnetic flux density norm to be expected at location where
* measurements have been made, expressed in Teslas (T).
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if calibrator is currently running.
*/
public void setGroundTruthMagneticFluxDensityNorm(final Double groundTruthMagneticFluxDensityNorm)
throws LockedException {
if (isRunning()) {
throw new LockedException();
}
internalSetGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm);
}
/**
* Sets ground truth magnetic flux density norm to be expected at location where
* measurements have been made.
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if calibrator is currently running.
*/
public void setGroundTruthMagneticFluxDensityNorm(final MagneticFluxDensity groundTruthMagneticFluxDensityNorm)
throws LockedException {
if (isRunning()) {
throw new LockedException();
}
if (groundTruthMagneticFluxDensityNorm != null) {
internalSetGroundTruthMagneticFluxDensityNorm(MagneticFluxDensityConverter.convert(
groundTruthMagneticFluxDensityNorm.getValue().doubleValue(),
groundTruthMagneticFluxDensityNorm.getUnit(),
MagneticFluxDensityUnit.TESLA));
} else {
internalSetGroundTruthMagneticFluxDensityNorm(null);
}
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return super.isReady() && getGroundTruthMagneticFluxDensityNorm() != null;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9266 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9310 |
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
oldC, oldSpeedX, oldSpeedY, oldSpeedZ, convertAccelerationToDouble(fx), convertAccelerationToDouble(fy),
convertAccelerationToDouble(fz),
convertAngularSpeedToDouble(angularRateX), convertAngularSpeedToDouble(angularRateY),
convertAngularSpeedToDouble(angularRateZ), result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9618 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9662 |
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(), oldC,
oldVx, oldVy, oldVz,
convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
convertAngularSpeedToDouble(angularRateX), convertAngularSpeedToDouble(angularRateY),
convertAngularSpeedToDouble(angularRateZ), result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9707 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9752 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
convertAngularSpeedToDouble(angularRateX),
convertAngularSpeedToDouble(angularRateY),
convertAngularSpeedToDouble(angularRateZ), result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java | 1552 |
| com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java | 1504 |
}
/**
* Indicates whether provided measurements are ready to be
* used for an update.
*
* @param measurements measurements to be checked.
* @return true if estimator is ready, false otherwise.
*/
public static boolean isUpdateMeasurementsReady(final Collection<GNSSMeasurement> measurements) {
return GNSSLeastSquaresPositionAndVelocityEstimator.isValidMeasurements(measurements);
}
/**
* Updates GNSS measurements of this estimator when new satellite measurements
* are available.
* Calls to this method will be ignored if interval between provided timestamp
* and last timestamp when Kalman filter was updated is less than epoch interval.
*
* @param measurements GNSS measurements to be updated.
* @param timestamp timestamp since epoch time when GNSS measurements were
* updated.
* @return true if measurements were updated, false otherwise.
* @throws LockedException if this estimator is already running.
* @throws NotReadyException if estimator is not ready for measurements updates.
* @throws INSGNSSException if estimation fails due to numerical instabilities.
*/
public boolean updateMeasurements(final Collection<GNSSMeasurement> measurements, final Time timestamp)
throws LockedException, NotReadyException, INSGNSSException {
return updateMeasurements(measurements, TimeConverter.convert(timestamp.getValue().doubleValue(),
timestamp.getUnit(), TimeUnit.SECOND));
}
/**
* Updates GNSS measurements of this estimator when new satellite measurements
* are available.
* Call to this method will be ignored if interval between provided timestamp
* and last timestamp when Kalman filter was updated is less than epoch interval.
*
* @param measurements GNSS measurements to be updated.
* @param timestamp timestamp expressed in seconds since epoch time when
* GNSS measurements were updated.
* @return true if measurements were updated, false otherwise.
* @throws LockedException if this estimator is already running.
* @throws NotReadyException if estimator is not ready for measurements updates.
* @throws INSGNSSException if estimation fails due to numerical instabilities.
*/
public boolean updateMeasurements(final Collection<GNSSMeasurement> measurements, final double timestamp)
throws LockedException, NotReadyException, INSGNSSException {
if (running) {
throw new LockedException();
}
if (!isUpdateMeasurementsReady(measurements)) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 5328 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5795 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6571 |
getBiasAsMatrix(ba);
fmeas.setElementAtIndex(0, fmeasX);
fmeas.setElementAtIndex(1, fmeasY);
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1641 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1663 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1285 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1255 |
return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3959 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4551 |
return evaluateGeneralWithGDependentCrossBiases(i, params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var m11 = result[3];
final var m21 = result[4];
final var m31 = result[5];
final var m12 = result[6];
final var m22 = result[7];
final var m32 = result[8];
final var m13 = result[9];
final var m23 = result[10];
final var m33 = result[11];
final var g11 = result[12]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2041 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3124 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1251 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2045 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3086 |
this.biasZ = convertAngularSpeed(biasZ);
}
/**
* Gets known gyroscope bias.
*
* @return known gyroscope bias.
*/
public AngularSpeedTriad getBiasAsTriad() {
return new AngularSpeedTriad(AngularSpeedUnit.RADIANS_PER_SECOND, biasX, biasY, biasZ);
}
/**
* Gets known gyroscope bias.
*
* @param result instance where result will be stored.
*/
public void getBiasAsTriad(final AngularSpeedTriad result) {
result.setValueCoordinatesAndUnit(biasX, biasY, biasZ, AngularSpeedUnit.RADIANS_PER_SECOND);
}
/**
* Sets known gyroscope bias.
*
* @param bias gyroscope bias to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setBias(final AngularSpeedTriad bias) throws LockedException {
if (running) {
throw new LockedException();
}
biasX = convertAngularSpeed(bias.getValueX(), bias.getUnit());
biasY = convertAngularSpeed(bias.getValueY(), bias.getUnit());
biasZ = convertAngularSpeed(bias.getValueZ(), bias.getUnit());
}
/**
* Gets initial x scaling factor of gyroscope.
*
* @return initial x scaling factor of gyroscope.
*/
@Override
public double getInitialSx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5799 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5895 |
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, biasX);
b.setElementAtIndex(1, biasY);
b.setElementAtIndex(2, biasZ); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 5277 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 3059 |
final var angularRateMeasZ2 = biasZ + m1.getElementAtIndex(2);
final var diffX = angularRateMeasX2 - angularRateMeasX1;
final var diffY = angularRateMeasY2 - angularRateMeasY1;
final var diffZ = angularRateMeasZ2 - angularRateMeasZ1;
return Math.sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);
} catch (final WrongSizeException e) {
return Double.MAX_VALUE;
}
}
/**
* Computes a preliminary solution for a subset of samples picked by a robust estimator.
*
* @param samplesIndices indices of samples picked by the robust estimator.
* @param solutions list where estimated preliminary solution will be stored.
*/
protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {
final var meas = new ArrayList<StandardDeviationFrameBodyKinematics>();
for (final var samplesIndex : samplesIndices) {
meas.add(this.measurements.get(samplesIndex));
}
try {
final var result = new PreliminaryResult();
result.estimatedMg = getInitialMg(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6575 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6675 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3500 |
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, bx);
b.setElementAtIndex(1, by);
b.setElementAtIndex(2, bz); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 5330 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6361 |
fmeas.setElementAtIndex(0, fmeasX);
fmeas.setElementAtIndex(1, fmeasY);
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6365 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 5128 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6677 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3502 |
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, bx);
b.setElementAtIndex(1, by);
b.setElementAtIndex(2, bz); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 5257 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 10944 |
final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
m1.add(mg);
final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
angularRateTrue.setElementAtIndex(0, angularRateTrueX);
angularRateTrue.setElementAtIndex(1, angularRateTrueY);
angularRateTrue.setElementAtIndex(2, angularRateTrueZ);
m1.multiply(angularRateTrue);
final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
fTrue.setElementAtIndex(0, fTrueX);
fTrue.setElementAtIndex(1, fTrueY);
fTrue.setElementAtIndex(2, fTrueZ);
final var m2 = gg.multiplyAndReturnNew(fTrue);
m1.add(m2);
final var angularRateMeasX2 = biasX + m1.getElementAtIndex(0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10445 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 3039 |
final var m1 = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
m1.add(mg);
final var angularRateTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
angularRateTrue.setElementAtIndex(0, angularRateTrueX);
angularRateTrue.setElementAtIndex(1, angularRateTrueY);
angularRateTrue.setElementAtIndex(2, angularRateTrueZ);
m1.multiply(angularRateTrue);
final var fTrue = new Matrix(BodyKinematics.COMPONENTS, 1);
fTrue.setElementAtIndex(0, fTrueX);
fTrue.setElementAtIndex(1, fTrueY);
fTrue.setElementAtIndex(2, fTrueZ);
final var m2 = gg.multiplyAndReturnNew(fTrue);
m1.add(m2);
final var angularRateMeasX2 = biasX + m1.getElementAtIndex(0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2715 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3498 |
bmeas.setElementAtIndex(0, bmeasX);
bmeas.setElementAtIndex(1, bmeasY);
bmeas.setElementAtIndex(2, bmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3714 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3983 |
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
biasX, biasY, biasZ, commonAxisUsed);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
biasX, biasY, biasZ, commonAxisUsed);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasY known y coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasZ known z coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2855 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3405 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2785 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3294 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3386 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3935 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3312 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3816 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4336 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5162 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3771 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 4038 |
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, biasX, biasY, biasZ, commonAxisUsed);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, biasX, biasY, biasZ, commonAxisUsed);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 6 samples.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3846 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4682 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, accelerometerBias,
accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 5402 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10514 |
innerCalibrator.setSequences(seqs);
innerCalibrator.calibrate();
result.estimatedMg = innerCalibrator.getEstimatedMg();
result.estimatedGg = innerCalibrator.getEstimatedGg();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4632 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4064 |
jacobian.setElementAt(9, 8, 1.0);
// propagated covariance is J * Cov * J'
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Internal method to perform general calibration.
*
* @throws AlgebraException if there are numerical errors.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
* @throws IOException if world magnetic model cannot be loaded.
*/
private void calibrateGeneral() throws AlgebraException, FittingException, com.irurueta.numerical.NotReadyException,
IOException {
// The magnetometer model is:
// mBmeas = ba + (I + Mm) * mBtrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// mBmeas = ba + (I + Mm) * mBtrue
// Hence:
// [mBmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [mBtruex]
// [mBmeasy] = [by] [0 1 0] [myx sy myz] [mBtruey]
// [mBmeasz] = [bz] [0 0 1] [mzx mzy sz ] [mBtruez]
// [mBmeasx] = [bx] + [1+sx mxy mxz ][mBtruex]
// [mBmeasy] [by] [myx 1+sy myz ][mBtruey]
// [mBmeasz] [bz] [mzx mzy 1+sz][mBtruez]
// mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy = by + myx * mBtruex + (1+sy) * mBtruey + myz * mBtruez
// mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + (1+sz) * mBtruez
// Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
// Reordering:
// mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy = by + myx * mBtruex + mBtruey + sy * mBtruey + myz * mBtruez
// mBmeasz = bz + mzx * mBtruex + mzy * mBtruey + mBtruez + sz * mBtruez
// mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy - mBtruey = by + myx * mBtruex + sy * mBtruey + myz * mBtruez
// mBmeasz - mBtruez = bz + mzx * mBtruex + mzy * mBtruey + sz * mBtruez
// [1 0 0 mBtruex 0 0 mBtruey mBtruez 0 0 0 0 ][bx ] = [mBmeasx - mBtruex]
// [0 1 0 0 mBtruey 0 0 0 mBtruex mBtruez 0 0 ][by ] [mBmeasy - mBtruey]
// [0 0 1 0 0 mBtruez 0 0 0 0 mBtruex mBtruey][bz ] [mBmeasz - mBtruez]
// [sx ]
// [sy ]
// [sz ]
// [mxy]
// [mxz]
// [myx]
// [myz]
// [mzx]
// [mzy]
fitter.setFunctionEvaluator(new LevenbergMarquardtMultiVariateFunctionEvaluator() {
@Override
public int getNumberOfDimensions() {
// Input points are true magnetic flux density coordinates
return BodyMagneticFluxDensity.COMPONENTS;
}
@Override
public int getNumberOfVariables() {
// The multivariate function returns the components of measured magnetic flux density
return BodyMagneticFluxDensity.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[GENERAL_UNKNOWNS];
initial[0] = initialHardIronX; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3123 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3708 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3631 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4210 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5007 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 473 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2982 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2982 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1693 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 4243 |
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases.
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5827 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3959 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4551 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5933 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2963 |
return evaluateGeneral(params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var m11 = result[3];
final var m21 = result[4];
final var m31 = result[5];
final var m12 = result[6];
final var m22 = result[7];
final var m32 = result[8];
final var m13 = result[9];
final var m23 = result[10];
final var m33 = result[11];
final var bias = new Matrix(BodyKinematics.COMPONENTS, 1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 707 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 711 |
final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 60 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 65 |
public abstract class RobustKnownBiasAndGravityNormAccelerometerCalibrator implements
AccelerometerNonLinearCalibrator, KnownBiasAccelerometerCalibrator,
OrderedStandardDeviationBodyKinematicsAccelerometerCalibrator, QualityScoredAccelerometerCalibrator {
/**
* Indicates whether by default a common z-axis is assumed for both the accelerometer
* and gyroscope.
*/
public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
/**
* Required minimum number of measurements when common z-axis is assumed.
*/
public static final int MINIMUM_MEASUREMENTS_COMMON_Z_AXIS =
BaseBiasGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;
/**
* Required minimum number of measurements for the general case.
*/
public static final int MINIMUM_MEASUREMENTS_GENERAL =
BaseBiasGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL;
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default.
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Contains a list of body kinematics measurements taken at a
* given position with different unknown orientations and containing the
* standard deviations of accelerometer and gyroscope measurements.
*/
protected List<StandardDeviationBodyKinematics> measurements;
/**
* Listener to be notified of events such as when calibration starts, ends or its
* progress significantly changes.
*/
protected RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1632 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 2290 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 2249 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2870 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 485 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3230 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4162 |
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return array containing x,y,z components of estimated accelerometer biases.
*/
@Override
public double[] getEstimatedBiases() {
return estimatedBiases;
}
/**
* Gets array containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where estimated accelerometer biases will be stored.
* @return true if result instance was updated, false otherwise (when estimation
* is not yet available).
*/
@Override
public boolean getEstimatedBiases(final double[] result) {
if (estimatedBiases != null) {
System.arraycopy(estimatedBiases, 0, result, 0, estimatedBiases.length);
return true;
} else {
return false;
}
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @return column matrix containing x,y,z components of estimated accelerometer
* biases
*/
@Override
public Matrix getEstimatedBiasesAsMatrix() {
return estimatedBiases != null ? Matrix.newFromArray(estimatedBiases) : null;
}
/**
* Gets column matrix containing x,y,z components of estimated accelerometer biases
* expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be stored.
* @return true if result was updated, false otherwise.
* @throws WrongSizeException if provided result instance has invalid size.
*/
@Override
public boolean getEstimatedBiasesAsMatrix(final Matrix result) throws WrongSizeException {
if (estimatedBiases != null) {
result.fromArray(estimatedBiases);
return true;
} else {
return false;
}
}
/**
* Gets x coordinate of estimated accelerometer bias expressed in meters per
* squared second (m/s^2).
*
* @return x coordinate of estimated accelerometer bias or null if not available.
*/
@Override
public Double getEstimatedBiasFx() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4626 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6747 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5662 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7875 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5162 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7238 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6179 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8346 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 65 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 76 |
public abstract class RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator implements
MagnetometerNonLinearCalibrator, KnownHardIronMagnetometerCalibrator,
OrderedStandardDeviationBodyMagneticFluxDensityMagnetometerCalibrator, QualityScoredMagnetometerCalibrator {
/**
* Indicates whether by default a common z-axis is assumed for the accelerometer,
* gyroscope and magnetometer.
*/
public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
/**
* Required minimum number of measurements when common z-axis is assumed.
*/
public static final int MINIMUM_MEASUREMENTS_COMMON_Z_AXIS =
BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;
/**
* Required minimum number of measurements for the general case.
*/
public static final int MINIMUM_MEASUREMENTS_GENERAL =
BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL;
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default.
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Contains a list of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*/
protected List<StandardDeviationBodyMagneticFluxDensity> measurements;
/**
* Listener to be notified of events such as when calibration starts, ends or its
* progress significantly changes.
*/
protected RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 65 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 76 |
public abstract class RobustKnownMagneticFluxDensityNormMagnetometerCalibrator implements
MagnetometerNonLinearCalibrator, UnknownHardIronNonLinearMagnetometerCalibrator,
OrderedStandardDeviationBodyMagneticFluxDensityMagnetometerCalibrator, QualityScoredMagnetometerCalibrator {
/**
* Indicates whether by default a common z-axis is assumed for the accelerometer,
* gyroscope and magnetometer.
*/
public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
/**
* Required minimum number of measurements when common z-axis is assumed.
*/
public static final int MINIMUM_MEASUREMENTS_COMMON_Z_AXIS =
BaseMagneticFluxDensityNormMagnetometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS;
/**
* Required minimum number of measurements for the general case.
*/
public static final int MINIMUM_MEASUREMENTS_GENERAL =
BaseMagneticFluxDensityNormMagnetometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL;
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default.
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Contains a list of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*/
protected List<StandardDeviationBodyMagneticFluxDensity> measurements;
/**
* Listener to be notified of events such as when calibration starts, ends or its
* progress significantly changes.
*/
protected RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 7985 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 6949 |
final var diff = groundTruthMagneticFluxDensityNorm - norm;
return diff * diff;
} catch (final AlgebraException e) {
return Double.MAX_VALUE;
}
}
/**
* Computes a preliminary solution for a subset of samples picked by a robust estimator.
*
* @param samplesIndices indices of samples picked by the robust estimator.
* @param solutions list where estimated preliminary solution will be stored.
*/
protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {
final var meas = new ArrayList<StandardDeviationBodyMagneticFluxDensity>();
for (final var samplesIndex : samplesIndices) {
meas.add(this.measurements.get(samplesIndex));
}
try {
final var result = new PreliminaryResult();
result.estimatedHardIron = getInitialHardIron();
result.estimatedMm = getInitialMm();
innerCalibrator.setInitialHardIron(result.estimatedHardIron);
innerCalibrator.setInitialMm(result.estimatedMm);
innerCalibrator.setCommonAxisUsed(commonAxisUsed);
innerCalibrator.setGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3135 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3885 |
jacobian.setElementAt(6, 5, 1.0);
// propagated covariance is J * Cov * J'
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Internal method to perform general calibration.
*
* @throws AlgebraException if there are numerical errors.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateGeneral() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException {
// The accelerometer model is:
// fmeas = ba + (I + Ma) * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// fmeas = ba + (I + Ma) * ftrue
// Hence:
// [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
// [fmeasy] = [by] [0 1 0] [myx sy myz] [ftruey]
// [fmeasz] = [bz] [0 0 1] [mzx mzy sz ] [ftruez]
// [fmeasx] = [bx] + [1+sx mxy mxz ][ftruex]
// [fmeasy] [by] [myx 1+sy myz ][ftruey]
// [fmeasz] [bz] [mzx mzy 1+sz][ftruez]
// fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + (1+sy) * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + (1+sz) * ftruez
// Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
// Reordering:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez
// fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy - ftruey - by = myx * ftruex + sy * ftruey + myz * ftruez
// fmeasz - ftruez - bz = mzx * ftruex + mzy * ftruey + sz * ftruez
// [ftruex 0 0 ftruey ftruez 0 0 0 0 ][sx ] = [fmeasx - ftruex - bx]
// [0 ftruey 0 0 0 ftruex ftruez 0 0 ][sy ] [fmeasy - ftruey - by]
// [0 0 ftruez 0 0 0 0 ftruex ftruey][sz ] [fmeasz - ftruez - bz]
// [mxy]
// [mxz]
// [myx]
// [myz]
// [mzx]
// [mzy]
fitter.setFunctionEvaluator(new LevenbergMarquardtMultiVariateFunctionEvaluator() {
@Override
public int getNumberOfDimensions() {
// Input points are true specific force coordinates
return BodyKinematics.COMPONENTS;
}
@Override
public int getNumberOfVariables() {
// The multivariate function returns the components of measured specific force
return BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[GENERAL_UNKNOWNS];
initial[0] = initialSx; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 4091 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4840 |
jacobian.setElementAt(2, 11, ftruey);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var sx = result[3];
final var sy = result[4];
final var sz = result[5];
final var mxy = result[6];
final var mxz = result[7];
final var myx = result[8];
final var myz = result[9];
final var mzx = result[10];
final var mzy = result[11];
if (estimatedBiases == null) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1577 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 406 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 790 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 799 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1286 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1581 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1256 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 410 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 409 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 412 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2011 |
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1741 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1761 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3467 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3530 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1673 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1705 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2018 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2010 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2014 |
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1642 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1664 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 735 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 845 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 216 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 854 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 628 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 739 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 612 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 221 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 218 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 220 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 933 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 940 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 931 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 936 |
}
/**
* Indicates whether inliers must be computed and kept.
*
* @return true if inliers must be computed and kept, false if inliers
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepInliersEnabled() {
return computeAndKeepInliers;
}
/**
* Specifies whether inliers must be computed and kept.
*
* @param computeAndKeepInliers true if inliers must be computed and kept,
* false if inliers only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepInliers = computeAndKeepInliers;
}
/**
* Indicates whether residuals must be computed and kept.
*
* @return true if residuals must be computed and kept, false if residuals
* only need to be computed but not kept.
*/
public boolean isComputeAndKeepResiduals() {
return computeAndKeepResiduals;
}
/**
* Specifies whether residuals must be computed and kept.
*
* @param computeAndKeepResiduals true if residuals must be computed and kept,
* false if residuals only need to be computed but not kept.
* @throws LockedException if calibrator is currently running.
*/
public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
if (running) {
throw new LockedException();
}
this.computeAndKeepResiduals = computeAndKeepResiduals;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 61 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 65 |
AccelerometerNonLinearCalibrator, KnownBiasAccelerometerCalibrator,
OrderedStandardDeviationFrameBodyKinematicsAccelerometerCalibrator,
QualityScoredAccelerometerCalibrator {
/**
* Indicates whether by default a common z-axis is assumed for both the accelerometer
* and gyroscope.
*/
public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
/**
* Required minimum number of measurements.
*/
public static final int MINIMUM_MEASUREMENTS = 4;
/**
* Indicates that by default a linear calibrator is used for preliminary solution estimation.
* The result obtained on each preliminary solution might be later refined.
*/
public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;
/**
* Indicates that by default preliminary solutions are refined.
*/
public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default using a non-linear calibrator
* (which uses a Levenberg-Marquardt fitter).
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Contains a list of body kinematics measurements taken at different
* frames (positions, orientations and velocities) and containing the standard
* deviations of accelerometer and gyroscope measurements.
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*/
protected List<StandardDeviationFrameBodyKinematics> measurements;
/**
* Listener to be notified of events such as when calibration starts, ends or its
* progress significantly changes.
*/
protected RobustKnownBiasAndFrameAccelerometerCalibratorListener listener; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2856 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2989 |
final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final Matrix bias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2988 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3534 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3057 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3601 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final Matrix bias, final Matrix initialMa,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3094 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3637 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3406 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3535 |
final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final Matrix bias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3901 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4059 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] bias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4551 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4706 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is smaller
* than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Acceleration groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2786 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2905 |
final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final Matrix bias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2904 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3414 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2969 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3479 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final Matrix bias, final Matrix initialMa,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3004 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3514 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix bias, final Matrix initialMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3295 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3415 |
final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
final Matrix bias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3761 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3910 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] bias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4363 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4512 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3387 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3519 |
final boolean commonAxisUsed, final double[] initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3518 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4063 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3586 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4129 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMa,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3622 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4164 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3936 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4064 |
final boolean commonAxisUsed, final double[] initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4427 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4587 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5077 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5232 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is smaller
* than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Acceleration groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3313 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3432 |
final boolean commonAxisUsed, final double[] initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3431 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3935 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3495 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3999 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMa,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3529 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4033 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix initialBias, final Matrix initialMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3817 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3936 |
final boolean commonAxisUsed, final double[] initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4273 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4426 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4865 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5015 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3823 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4109 |
jacobian.setElementAt(0, 7, 0.0);
jacobian.setElementAt(0, 8, 0.0);
jacobian.setElementAt(0, 9, ftruex);
jacobian.setElementAt(0, 10, ftruey);
jacobian.setElementAt(0, 11, ftruez);
jacobian.setElementAt(0, 12, 0.0);
jacobian.setElementAt(0, 13, 0.0);
jacobian.setElementAt(0, 14, 0.0);
jacobian.setElementAt(0, 15, 0.0);
jacobian.setElementAt(0, 16, 0.0);
jacobian.setElementAt(0, 17, 0.0);
jacobian.setElementAt(1, 0, 0.0);
jacobian.setElementAt(1, 1, omegatruey); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3843 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4129 |
jacobian.setElementAt(1, 8, 0.0);
jacobian.setElementAt(1, 9, 0.0);
jacobian.setElementAt(1, 10, 0.0);
jacobian.setElementAt(1, 11, 0.0);
jacobian.setElementAt(1, 12, ftruex);
jacobian.setElementAt(1, 13, ftruey);
jacobian.setElementAt(1, 14, ftruez);
jacobian.setElementAt(1, 15, 0.0);
jacobian.setElementAt(1, 16, 0.0);
jacobian.setElementAt(1, 17, 0.0);
jacobian.setElementAt(2, 0, 0.0);
jacobian.setElementAt(2, 1, 0.0);
jacobian.setElementAt(2, 2, omegatruez); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 3626 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 3767 |
final double[] hardIron,
final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron matrix is not 3x1.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 5055 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 5218 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron array does not have length 3,
* or if provided quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] hardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3124 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3265 |
final boolean commonAxisUsed, final double[] hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3264 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3849 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3339 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3924 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final Matrix initialMm,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3379 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3964 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron, final Matrix initialMm,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, initialMm);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3709 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3850 |
final boolean commonAxisUsed, final double[] hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4430 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4589 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] hardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5101 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5263 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] hardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4137 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4278 |
final double[] initialHardIron,
final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron matrix is not 3x1.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 5568 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 5731 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron array does not have length 3,
* or if provided quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] initialHardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3632 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3773 |
final boolean commonAxisUsed, final double[] initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix initialHardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3772 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4351 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3847 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4426 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix initialHardIron, final Matrix initialMm,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3887 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4466 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix initialHardIron, final Matrix initialMm,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, initialMm);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4211 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4352 |
final boolean commonAxisUsed, final double[] initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix initialHardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4918 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5077 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] initialHardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5584 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5747 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 2142 |
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 2169 |
final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
estimateKinematics(timeInterval, frame, oldC,
SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param frame body ECEF frame containing current position, velocity and
* body-to-ECEF frame coordinate transformation.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 2142 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1210 |
final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
estimateKinematics(timeInterval, frame, oldC,
SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param frame body ECEF frame containing current position, velocity and
* body-to-ECEF frame coordinate transformation.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1210 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1237 |
final double timeInterval, final ECIFrame frame, final CoordinateTransformation oldC,
final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
estimateKinematics(timeInterval, frame, oldC,
SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param frame body ECI frame containing current position, velocity and
* body-to-ECI frame coordinate transformation.
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx x coordinate of previous velocity of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldVy y coordinate of previous velocity of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldVz z coordinate of previous velocity of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param result instance where estimated body kinematics will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinates transformation matrices
* are not ECI frame valid.
*/
public static void estimateKinematics(
final Time timeInterval, final ECIFrame frame, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8981 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9022 |
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final ECEFVelocity oldVelocity, final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(), oldC,
oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(), fx, fy, fz,
convertAngularSpeedToDouble(angularRateX),
convertAngularSpeedToDouble(angularRateY),
convertAngularSpeedToDouble(angularRateZ), result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 188 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 193 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 190 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 192 |
final RobustKnownFrameAccelerometerCalibratorListener listener) {
super(measurements, commonAxisUsed, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 842 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 790 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 212 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 849 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java | 796 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 738 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1669 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 218 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1701 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 215 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 933 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 927 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 932 |
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1791 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1694 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 456 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1811 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1713 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1633 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3515 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 460 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3580 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 460 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2061 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2058 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2063 |
final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public double[] getQualityScores() {
return qualityScores;
}
@Override
public double getThreshold() {
return stopThreshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.this.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 913 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 860 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 284 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 922 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 869 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 807 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1740 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 289 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1772 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 286 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1001 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 999 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1004 |
final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2742 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2964 |
final double biasX, final double biasY, final double biasZ,
final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param biasX known x coordinate of accelerometer bias.
* @param biasY known y coordinate of accelerometer bias.
* @param biasZ known z coordinate of accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/bias/BodyKinematicsBiasEstimator.java | 1133 |
| com/irurueta/navigation/inertial/calibration/bias/BodyMagneticFluxDensityBiasEstimator.java | 1755 |
}
/**
* Gets time interval between body kinematics (IMU acceleration + gyroscope)
* samples.
*
* @return time interval between body kinematics samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between body kinematics (IMU acceleration + gyroscope)
* samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between body kinematics (IMU acceleration + gyroscope)
* samples.
*
* @param timeInterval time interval between body kinematics samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(convertTime(timeInterval));
}
/**
* Gets current body position expressed in ECEF coordinates.
*
* @return current body position expressed in ECEF coordinates.
*/
public ECEFPosition getEcefPosition() {
return frame.getECEFPosition();
}
/**
* Gets current body position expressed in ECEF coordinates.
*
* @param result instance where current body position will be stored.
*/
public void getEcefPosition(final ECEFPosition result) {
frame.getECEFPosition(result);
}
/**
* Sets current body position expressed in ECEF coordinates.
*
* @param position current body position to be set.
* @throws LockedException if estimator is currently running.
*/
public void setEcefPosition(final ECEFPosition position) throws LockedException {
if (running) {
throw new LockedException();
}
frame.setPosition(position); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4465 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4548 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
initialGg);
default -> new PROMedSRobustEasyGyroscopeCalibrator(qualityScores, sequences, initialBias, initialMg,
initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a solution.
* This must be 3x1 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2807 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3024 |
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param biasX known x coordinate of gyroscope bias.
* @param biasY known y coordinate of gyroscope bias.
* @param biasZ known z coordinate of gyroscope bias.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final AngularSpeed biasX, final AngularSpeed biasY, final AngularSpeed biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3975 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4061 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
qualityScores, sequences, initialBias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
qualityScores, sequences, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* sequence. The larger the score value the better
* the quality of the sequence.
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a solution.
* This must be 3x1 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size or if provided
* quality scores length is smaller
* than 10.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final double[] qualityScores,
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4523 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6630 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4626 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7875 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5559 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7758 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5662 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6747 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5057 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7122 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5162 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8346 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6074 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8230 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6179 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7238 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 906 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 910 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 882 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 886 |
super(groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Gets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not when testing possible solutions.
* The threshold refers to the amount of error on norm between measured specific forces and the
* ones generated with estimated calibration parameters provided for each sample.
*
* @param threshold threshold to determine whether samples are inliers or not.
* @throws IllegalArgumentException if provided value is equal or less than zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates magnetometer calibration parameters containing hard-iron
* bias and soft-iron scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 1841 |
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 1906 |
public double[] fixAndReturnNew(
final double measuredAngularRateX, final double measuredAngularRateY, final double measuredAngularRateZ,
final double trueFx, final double trueFy, final double trueFz,
final double biasX, final double biasY, final double biasZ,
final double sx, final double sy, final double sz,
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy,
final double g11, final double g21, final double g31,
final double g12, final double g22, final double g32,
final double g13, final double g23, final double g33) throws AlgebraException {
final var result = new double[BodyKinematics.COMPONENTS]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 973 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1285 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 4091 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4563 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4840 |
jacobian.setElementAt(2, 11, ftruey);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var sx = result[3];
final var sy = result[4];
final var sz = result[5];
final var mxy = result[6];
final var mxz = result[7];
final var myx = result[8];
final var myz = result[9];
final var mzx = result[10];
final var mzy = result[11]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1266 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 977 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2917 |
biasZ = convertAcceleration(bias.getValueZ(), bias.getUnit());
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known accelerometer bias.
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3900 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4550 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3941 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4590 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3760 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4362 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3800 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4402 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4426 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5076 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4468 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5116 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4272 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4864 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4312 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4904 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5236 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2372 |
measAngularRateZ = point[2];
gradientEstimator.gradient(params, derivatives);
return evaluateGeneral(params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var m11 = result[0];
final var m21 = result[1];
final var m31 = result[2];
final var m12 = result[3];
final var m22 = result[4];
final var m32 = result[5];
final var m13 = result[6];
final var m23 = result[7];
final var m33 = result[8];
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 101 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 124 |
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default using a non-linear calibrator
* (which uses a Levenberg-Marquardt fitter).
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Known x-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasX;
/**
* Known y-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasY;
/**
* Known z-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasZ;
/**
* Known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSx;
/**
* Known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSy;
/**
* Known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSz;
/**
* Known accelerometer x-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxy;
/**
* Know accelerometer x-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxz;
/**
* Known accelerometer y-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyx;
/**
* Known accelerometer y-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyz;
/**
* Known accelerometer z-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzx;
/**
* Known accelerometer z-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzy;
/**
* Initial x-coordinate of gyroscope bias to be used to find a solution.
* This is expressed in radians per second (rad/s).
*/
private double initialBiasX; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 96 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 129 |
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default using a non-linear calibrator
* (which uses a Levenberg-Marquardt fitter).
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Known x-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasX;
/**
* Known y-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasY;
/**
* Known z-coordinate of accelerometer bias to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
* This is expressed in meters per squared second (m/s^2).
*/
private double accelerometerBiasZ;
/**
* Known accelerometer x scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSx;
/**
* Known accelerometer y scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSy;
/**
* Known accelerometer z scaling factor to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerSz;
/**
* Known accelerometer x-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxy;
/**
* Know accelerometer x-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMxz;
/**
* Known accelerometer y-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyx;
/**
* Known accelerometer y-z cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMyz;
/**
* Known accelerometer z-x cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzx;
/**
* Known accelerometer z-y cross coupling error to be used to fix measured
* specific force and find cross biases introduced by the accelerometer.
*/
private double accelerometerMzy;
/**
* X-coordinate of gyroscope known bias expressed in radians per second
* (rad/s).
*/
private double biasX; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5295 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6332 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7634 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8763 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5448 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6485 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7470 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8599 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa,
listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5816 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6833 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8110 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9220 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5966 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6983 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7949 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9059 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5672 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3215 |
measAngularRateZ = point[2];
gradientEstimator.gradient(params, derivatives);
return evaluateCommonAxis(params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var m11 = result[3];
final var m12 = result[4];
final var m22 = result[5];
final var m13 = result[6];
final var m23 = result[7];
final var m33 = result[8];
final var mb = new Matrix(BodyKinematics.COMPONENTS, 1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4429 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5100 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4471 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5142 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4917 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5583 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4959 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5625 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1716 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1881 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1785 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 547 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1901 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1807 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 869 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 981 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 928 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 352 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 990 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 937 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1428 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1722 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1399 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3607 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 551 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3670 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 764 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 875 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 750 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1808 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 357 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1840 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2154 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2150 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1071 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1067 |
inliersData = null;
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3169 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4468 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5359 |
mm.setElementAtIndex(5, 0.0);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33);
final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mg.setElementAtIndex(0, g11);
mg.setElementAtIndex(1, g21);
mg.setElementAtIndex(2, g31);
mg.setElementAtIndex(3, g12);
mg.setElementAtIndex(4, g22);
mg.setElementAtIndex(5, g32);
mg.setElementAtIndex(6, g13);
mg.setElementAtIndex(7, g23);
mg.setElementAtIndex(8, g33);
setResult(mm, mg); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3440 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4767 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5014 |
mm.setElementAtIndex(5, m32);
mm.setElementAtIndex(6, m13);
mm.setElementAtIndex(7, m23);
mm.setElementAtIndex(8, m33);
final var mg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
mg.setElementAtIndex(0, g11);
mg.setElementAtIndex(1, g21);
mg.setElementAtIndex(2, g31);
mg.setElementAtIndex(3, g12);
mg.setElementAtIndex(4, g22);
mg.setElementAtIndex(5, g32);
mg.setElementAtIndex(6, g13);
mg.setElementAtIndex(7, g23);
mg.setElementAtIndex(8, g33);
setResult(mm, mg); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3671 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5007 |
return 2 * BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[COMMON_Z_AXIS_UNKNOWNS];
// upper diagonal cross coupling errors M
var k = 0;
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5370 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5433 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6108 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6171 |
private void setInputDataWithGDependentCrossBiases()
throws AlgebraException, InvalidSourceAndDestinationFrameTypeException {
// compute reference frame at current position
final var nedPosition = getNedPosition();
final var nedC = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
final var nedFrame = new NEDFrame(nedPosition, nedC);
final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
final var refKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
ecefFrame);
final var refAngularRateX = refKinematics.getAngularRateX();
final var refAngularRateY = refKinematics.getAngularRateY();
final var refAngularRateZ = refKinematics.getAngularRateZ();
final var w2 = turntableRotationRate * turntableRotationRate;
final var numMeasurements = measurements.size();
final var x = new Matrix(numMeasurements, 2 * BodyKinematics.COMPONENTS); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4099 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4496 |
result[2] = bz + omegatruez + sz * omegatruez
+ g31 * ftruex + g32 * ftruey + g33 * ftruez;
jacobian.setElementAt(0, 0, 1.0);
jacobian.setElementAt(0, 1, 0.0);
jacobian.setElementAt(0, 2, 0.0);
jacobian.setElementAt(0, 3, omegatruex);
jacobian.setElementAt(0, 4, 0.0);
jacobian.setElementAt(0, 5, 0.0);
jacobian.setElementAt(0, 6, omegatruey);
jacobian.setElementAt(0, 7, omegatruez);
jacobian.setElementAt(0, 8, 0.0);
jacobian.setElementAt(0, 9, ftruex); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 5885 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10515 |
innerCalibrator.getEstimatedBiases(result.estimatedBiases);
result.estimatedMg = innerCalibrator.getEstimatedMg();
result.estimatedGg = innerCalibrator.getEstimatedGg();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 5403 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 11018 |
innerCalibrator.calibrate();
result.estimatedMg = innerCalibrator.getEstimatedMg();
result.estimatedGg = innerCalibrator.getEstimatedGg();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = sequences.size(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4523 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7758 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5559 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6630 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator(position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5057 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8230 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6074 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7122 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 1281 |
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 1366 |
result.getBuffer());
}
/**
* Fixes provided measured angular rate values by undoing the errors
* introduced by the gyroscope model to restore the true angular
* rate.
*
* @param measuredAngularRateX x-coordinate of measured angular rate
* expressed in radians per second (rad/s).
* @param measuredAngularRateY y-coordinate of measured angular rate
* expressed in radians per second (rad/s).
* @param measuredAngularRateZ z-coordinate of measured angular rate
* expressed in radians per second (rad/s).
* @param trueFx x-coordinate of true (i.e. fixed)
* specific force expressed in meters per
* squared second (m/s^2).
* @param trueFy y-coordinate of true (i.e. fixed)
* specific force expressed in meters per
* squared second (m/s^2).
* @param trueFz z-coordinate of true (i.e. fixed)
* specific force expressed in meters per
* squared second (m/s^2).
* @param biasX x-coordinate of bias expressed in
* meters per squared second (m/s^2).
* @param biasY y-coordinate of bias expressed in
* meters per squared second (m/s^2).
* @param biasZ z-coordinate of bias expressed in
* meters per squared second (m/s^2).
* @param sx initial x scaling factor.
* @param sy initial y scaling factor.
* @param sz initial z scaling factor.
* @param mxy initial x-y cross coupling error.
* @param mxz initial x-z cross coupling error.
* @param myx initial y-x cross coupling error.
* @param myz initial y-z cross coupling error.
* @param mzx initial z-x cross coupling error.
* @param mzy initial z-y cross coupling error.
* @param g11 element 1,1 of g-dependant cross biases.
* @param g21 element 2,1 of g-dependant cross biases.
* @param g31 element 3,1 of g-dependant cross biases.
* @param g12 element 1,2 of g-dependant cross biases.
* @param g22 element 2,2 of g-dependant cross biases.
* @param g32 element 3,2 of g-dependant cross biases.
* @param g13 element 1,3 of g-dependant cross biases.
* @param g23 element 2,3 of g-dependant cross biases.
* @param g33 element 3,3 of g-dependant cross biases.
* @param result instance where restored true angular rate
* will be stored. Must have length 3.
* @throws AlgebraException if there are numerical instabilities.
* @throws IllegalArgumentException if any of the provided parameters
* does not have proper size.
*/
public void fix(
final double measuredAngularRateX, final double measuredAngularRateY, final double measuredAngularRateZ,
final double trueFx, final double trueFy, final double trueFz,
final double biasX, final double biasY, final double biasZ,
final double sx, final double sy, final double sz,
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy,
final double g11, final double g21, final double g31,
final double g12, final double g22, final double g32,
final double g13, final double g23, final double g33, final double[] result) throws AlgebraException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1347 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1457 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3375 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 878 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 986 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 4164 |
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var fMeasX = measuredKinematics.getFx();
final var fMeasY = measuredKinematics.getFy();
final var fMeasZ = measuredKinematics.getFz();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2670 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2898 |
final double biasX, final double biasY, final double biasZ,
final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasY known y coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasZ known z coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 5349 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 5435 |
this.biasZ = convertAcceleration(biasZ);
}
/**
* Internally sets known accelerometer bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known accelerometer bias.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
private void internalSetBias(final double[] bias) {
if (bias.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
biasX = bias[0];
biasY = bias[1];
biasZ = bias[2];
}
/**
* Internally sets known accelerometer bias as a column matrix.
* Matrix values are expressed in meters per squared second (m/s^2).
*
* @param bias accelerometer bias to be set.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
private void internalSetBias(final Matrix bias) {
if (bias.getRows() != BodyKinematics.COMPONENTS || bias.getColumns() != 1) {
throw new IllegalArgumentException();
}
biasX = bias.getElementAtIndex(0);
biasY = bias.getElementAtIndex(1);
biasZ = bias.getElementAtIndex(2);
}
/**
* Converts acceleration value and unit to meters per squared second.
*
* @param value acceleration value.
* @param unit unit of acceleration value.
* @return converted value.
*/
private static double convertAcceleration(final double value, final AccelerationUnit unit) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1930 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1889 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1975 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1936 |
setGroundTruthGravityNorm(groundTruthGravityNorm != null ? convertAcceleration(groundTruthGravityNorm) : null);
}
/**
* Gets a list of body kinematics measurements taken at a
* given position with different unknown orientations and containing the
* standard deviations of accelerometer and gyroscope measurements.
*
* @return list of body kinematics measurements.
*/
@Override
public List<StandardDeviationBodyKinematics> getMeasurements() {
return measurements;
}
/**
* Sets a list of body kinematics measurements taken at a
* given position with different unknown orientations and containing the
* standard deviations of accelerometer and gyroscope measurements.
*
* @param measurements list of body kinematics measurements.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final List<StandardDeviationBodyKinematics> measurements) throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public AccelerometerCalibratorMeasurementType getMeasurementType() {
return AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @return true if z-axis is assumed to be common for accelerometer and gyroscope,
* false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Ma matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for accelerometer
* and gyroscope, false otherwise.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this estimator.
*
* @return listener to handle events raised by this estimator.
*/
public RobustKnownBiasAndGravityNormAccelerometerCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3823 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4474 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3943 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4099 |
final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4058 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4705 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4098 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4744 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4178 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4822 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, initialMa);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4591 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4745 |
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is smaller
* than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Acceleration groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3689 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4288 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or quality scores array is smaller than 10
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3802 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3949 |
final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3909 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4511 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3948 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4550 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4025 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4627 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, initialMa);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3
* or if quality scores array is smaller than 10
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4403 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4551 |
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4347 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4999 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4470 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4627 |
final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4586 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5231 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4625 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5269 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final Matrix initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4705 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5347 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, initialMa);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5118 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5271 |
final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is smaller
* than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Acceleration groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4200 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4792 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4314 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4465 |
final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4425 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5014 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4463 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5052 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final Matrix initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4540 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5129 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, initialMa);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3
* or if quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4906 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5054 |
final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java | 570 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 591 |
public MSACRobustEasyGyroscopeCalibrator(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener) {
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java | 555 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 575 |
public MSACRobustKnownBiasEasyGyroscopeCalibrator(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix bias,
final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener) {
super(sequences, commonAxisUsed, estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias,
accelerometerMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3914 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4686 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2736 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2959 |
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed,
listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed,
listener);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed,
listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed,
listener);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3424 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4202 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 597 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1891 |
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Gets array containing x,y,z components of estimated magnetometer
* hard-iron biases expressed in Teslas (T).
*
* @return array containing x,y,z components of estimated magnetometer
* hard-iron biases.
*/
@Override
public double[] getEstimatedHardIron() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2030 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2142 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2058 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 2170 |
initialSz = initialMm.getElementAtIndex(8);
}
/**
* Gets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @return collection of body magnetic flux density measurements at
* a known position and timestamp with unknown orientations.
*/
@Override
public List<StandardDeviationBodyMagneticFluxDensity> getMeasurements() {
return measurements;
}
/**
* Sets collection of body magnetic flux density measurements taken
* at a given position with different unknown orientations and containing the
* standard deviation of magnetometer measurements.
*
* @param measurements collection of body magnetic flux density
* measurements at a known position and timestamp
* with unknown orientations.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setMeasurements(final List<StandardDeviationBodyMagneticFluxDensity> measurements)
throws LockedException {
if (running) {
throw new LockedException();
}
this.measurements = measurements;
}
/**
* Indicates the type of measurement used by this calibrator.
*
* @return type of measurement used by this calibrator.
*/
@Override
public MagnetometerCalibratorMeasurementType getMeasurementType() {
return MagnetometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_MAGNETIC_FLUX_DENSITY;
}
/**
* Indicates whether this calibrator requires ordered measurements in a
* list or not.
*
* @return true if measurements must be ordered, false otherwise.
*/
@Override
public boolean isOrderedMeasurementsRequired() {
return true;
}
/**
* Indicates whether z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer.
* When enabled, this eliminates 3 variables from Mm (soft-iron) matrix.
*
* @return true if z-axis is assumed to be common for accelerometer,
* gyroscope and magnetometer, false otherwise.
*/
@Override
public boolean isCommonAxisUsed() {
return commonAxisUsed;
}
/**
* Specifies whether z-axis is assumed to be common for accelerometer and
* gyroscope.
* When enabled, this eliminates 3 variables from Mm matrix.
*
* @param commonAxisUsed true if z-axis is assumed to be common for
* accelerometer, gyroscope and magnetometer, false
* otherwise.
* @throws LockedException if estimator is currently running.
*/
@Override
public void setCommonAxisUsed(final boolean commonAxisUsed) throws LockedException {
if (running) {
throw new LockedException();
}
this.commonAxisUsed = commonAxisUsed;
}
/**
* Gets listener to handle events raised by this calibrator.
*
* @return listener to handle events raised by this calibrator.
*/
public RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener getListener() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 4320 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 4470 |
final boolean commonAxisUsed, final double[] hardIron,
final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores length
* is smaller than 10 samples.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 5097 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 5260 |
final double[] hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron array does not have length 3,
* or if provided quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] hardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4349 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5020 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4473 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4631 |
final double[] hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4588 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5262 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4629 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5300 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final Matrix hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4712 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5383 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
final Matrix initialMm, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, initialMm);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, initialMm);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5144 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5302 |
final double[] hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4832 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4982 |
final boolean commonAxisUsed, final double[] initialHardIron,
final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores length
* is smaller than 10 samples.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix initialHardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 5610 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 5773 |
final double[] initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron array does not have length 3,
* or if provided quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] initialHardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4837 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5503 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4961 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5119 |
final double[] initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5076 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5746 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5117 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5784 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final Matrix initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5200 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5867 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
final Matrix initialMm, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, initialMm);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, initialMm);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5627 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5786 |
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] initialHardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 2142 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1237 |
final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
estimateKinematics(timeInterval, frame, oldC,
SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs.
* @param frame body ECEF frame containing current position, velocity and
* body-to-ECEF frame coordinate transformation.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 2169 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1210 |
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
final Speed oldVx, final Speed oldVy, final Speed oldVz, final BodyKinematics result) {
estimateKinematics(timeInterval, frame, oldC,
SpeedConverter.convert(oldVx.getValue().doubleValue(), oldVx.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVy.getValue().doubleValue(), oldVy.getUnit(), SpeedUnit.METERS_PER_SECOND),
SpeedConverter.convert(oldVz.getValue().doubleValue(), oldVz.getUnit(), SpeedUnit.METERS_PER_SECOND),
result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param c body-to-ECEF frame coordinate transformation matrix.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param vx velocity of body frame x coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vy velocity of body frame y coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param vz velocity of body frame z coordinate with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per
* second (m/s).
* @param oldVx previous velocity of body frame x coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVy previous velocity of body frame y coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param oldVz previous velocity of body frame z coordinate with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters
* per second (m/s).
* @param position body position expressed in meters (m) with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final double timeInterval, final CoordinateTransformation c, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8537 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8583 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8892 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8939 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(), fx, fy, fz,
convertAngularSpeedToDouble(angularRateX),
convertAngularSpeedToDouble(angularRateY),
convertAngularSpeedToDouble(angularRateZ), result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 5831 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4555 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5313 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5937 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 2967 |
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var m11 = result[3];
final var m21 = result[4];
final var m31 = result[5];
final var m12 = result[6];
final var m22 = result[7];
final var m32 = result[8];
final var m13 = result[9];
final var m23 = result[10];
final var m33 = result[11];
final var bias = new Matrix(BodyKinematics.COMPONENTS, 1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1404 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 930 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1439 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 984 |
fillMa(sx, sy, sz, mxy, mxz, 0.0, myz, 0.0, 0.0);
}
/**
* Internal method to perform general calibration.
*
* @throws AlgebraException if there are numerical errors.
*/
private void calibrateGeneral() throws AlgebraException {
// The accelerometer model is:
// fmeas = ba + (I + Ma) * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// fmeas = ba + (I + Ma) * ftrue
// Hence:
// [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
// [fmeasy] = [by] [0 1 0] [myx sy myz] [ftruey]
// [fmeasz] = [bz] [0 0 1] [mzx mzy sz ] [ftruez]
// [fmeasx] = [bx] + [1+sx mxy mxz ][ftruex]
// [fmeasy] [by] [myx 1+sy myz ][ftruey]
// [fmeasz] [bz] [mzx mzy 1+sz][ftruez]
// fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + (1+sy) * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + (1+sz) * ftruez
// Where the unknowns are: sx, sy, sz, mxy mxz, myx, myz, mzx, mzy
// Reordering:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez
// fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy - ftruey - by = myx * ftruex + sy * ftruey + myz * ftruez
// fmeasz - ftruez - bz = mzx * ftruex + mzy * ftruey + sz * ftruez
// [ftruex 0 0 ftruey ftruez 0 0 0 0 ][sx ] = [fmeasx - ftruex - bx]
// [0 ftruey 0 0 0 ftruex ftruez 0 0 ][sy ] [fmeasy - ftruey - by]
// [0 0 ftruez 0 0 0 0 ftruex ftruey][sz ] [fmeasz - ftruez - bz]
// [mxy]
// [mxz]
// [myx]
// [myz]
// [mzx]
// [mzy]
final var expectedKinematics = new BodyKinematics();
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, GENERAL_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var fMeasX = measuredKinematics.getFx(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1314 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1333 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2421 |
}
/**
* Gets known accelerometer bias as a column matrix.
*
* @return known accelerometer bias as a column matrix.
*/
@Override
public Matrix getBiasAsMatrix() {
Matrix result;
try {
result = new Matrix(BodyKinematics.COMPONENTS, 1);
getBiasAsMatrix(result);
} catch (final WrongSizeException ignore) {
// never happens
result = null;
}
return result;
}
/**
* Gets known accelerometer bias as a column matrix.
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void getBiasAsMatrix(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != 1) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, biasX);
result.setElementAtIndex(1, biasY);
result.setElementAtIndex(2, biasZ);
}
/**
* Sets known accelerometer bias as a column matrix.
*
* @param bias accelerometer bias to be set.
* @throws LockedException if calibrator is currently running
* @throws IllegalArgumentException if provided matrix is not 3x1.
*/
@Override
public void setBias(final Matrix bias) throws LockedException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3602 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3880 |
final double biasX, final double biasY, final double biasZ,
final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
listener);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
biasX, biasY, biasZ, listener);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
biasX, biasY, biasZ, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param biasX known x coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasY known y coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasZ known z coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final double[] qualityScores, final double biasX, final double biasY, final double biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3984 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4784 |
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4139 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4632 |
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3841 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4590 |
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
* quality scores array is smaller than 13
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3988 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4443 |
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3
* or if quality scores array is smaller than 10
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4512 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5310 |
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4667 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5159 |
final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4355 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5093 |
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
* quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4504 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4947 |
final Matrix initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3
* or if quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1429 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1400 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 765 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 751 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 554 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 554 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2159 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2158 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 358 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 357 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1077 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1076 |
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3660 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3936 |
final double biasX, final double biasY, final double biasZ,
final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, biasX, biasY, biasZ, listener);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, biasX, biasY, biasZ, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param biasX known x coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 6 samples.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final double[] qualityScores, final double biasX, final double biasY, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 7504 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 6995 |
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationBodyMagneticFluxDensity>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierMeasurements.add(measurements.get(i));
}
}
try {
innerCalibrator.setHardIronCoordinates(hardIronX, hardIronY, hardIronZ); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4515 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5343 |
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4672 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5186 |
final Matrix hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5003 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5827 |
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5160 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5669 |
final Matrix initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17695 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17750 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 18719 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 18774 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19197 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19252 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19690 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19742 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20168 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20220 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20649 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20704 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20753 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20802 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20851 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20900 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21299 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21348 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21493 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21542 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1290 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 818 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1304 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 839 |
}
/**
* Internal method to perform calibration when common z-axis is assumed for both
* the accelerometer and gyroscope.
*
* @throws AlgebraException if there are numerical errors.
*/
private void calibrateCommonAxis() throws AlgebraException {
// The accelerometer model is:
// fmeas = ba + (I + Ma) * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// fmeas = ba + (I + Ma) * ftrue
// Hence:
// [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
// [fmeasy] = [by] [0 1 0] [myx sy myz] [ftruey]
// [fmeasz] = [bz] [0 0 1] [mzx mzy sz ] [ftruez]
// where myx = mzx = mzy = 0
// Hence:
// [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
// [fmeasy] = [by] [0 1 0] [0 sy myz] [ftruey]
// [fmeasz] = [bz] [0 0 1] [0 0 sz ] [ftruez]
// [fmeasx] = [bx] + [1+sx mxy mxz ][ftruex]
// [fmeasy] [by] [0 1+sy myz ][ftruey]
// [fmeasz] [bz] [0 0 1+sz][ftruez]
// fmeasx = bx + (1+sx) * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + (1+sy) * ftruey + myz * ftruez
// fmeasz = bz + (1+sz) * ftruez
// Where the unknowns are: sx, sy, sz, mxy mxz, myz
// Reordering:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + ftruez + sz * ftruez
// fmeasx - ftruex - bx = sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy - ftruey - by = sy * ftruey + myz * ftruez
// fmeasz - ftruez - bz = sz * ftruez
// [ftruex 0 0 ftruey ftruez 0 ][sx ] = [fmeasx - ftruex - bx]
// [0 ftruey 0 0 0 ftruez][sy ] [fmeasy - ftruey - by]
// [0 0 ftruez 0 0 0 ][sz ] [fmeasz - ftruez - bz]
// [mxy]
// [mxz]
// [myz]
final var expectedKinematics = new BodyKinematics();
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS);
final var b = new Matrix(rows, 1);
var i = 0;
for (final var measurement : measurements) {
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
ECEFKinematicsEstimator.estimateKinematics(timeInterval, ecefFrame, previousEcefFrame, expectedKinematics);
final var fMeasX = measuredKinematics.getFx(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3098 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3844 |
if (estimatedMa == null) {
estimatedMa = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedMa.initialize(0.0);
}
estimatedMa.setElementAt(0, 0, sx);
estimatedMa.setElementAt(0, 1, mxy);
estimatedMa.setElementAt(1, 1, sy);
estimatedMa.setElementAt(0, 2, mxz);
estimatedMa.setElementAt(1, 2, myz);
estimatedMa.setElementAt(2, 2, sz);
estimatedCovariance = fitter.getCovar();
// propagate covariance matrix so that all parameters are taken into
// account in the order: sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy
// We define a lineal function mapping original parameters for the common
// axis case to the general case
// [sx'] = [1 0 0 0 0 0][sx]
// [sy'] [0 1 0 0 0 0][sy]
// [sz'] [0 0 1 0 0 0][sz]
// [mxy'] [0 0 0 1 0 0][mxy]
// [mxz'] [0 0 0 0 1 0][mxz]
// [myx'] [0 0 0 0 0 0][myz]
// [myz'] [0 0 0 0 0 1]
// [mzx'] [0 0 0 0 0 0]
// [mzy'] [0 0 0 0 0 0]
// As defined in com.irurueta.statistics.MultivariateNormalDist,
// if we consider the jacobian of the lineal application the matrix shown
// above, then covariance can be propagated as follows
final var jacobian = Matrix.identity(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
jacobian.setElementAt(5, 5, 0.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1717 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1882 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1786 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 548 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1902 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1808 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 870 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 982 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 929 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 353 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 991 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 938 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1723 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3608 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 552 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3671 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 876 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1809 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 358 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1841 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 554 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 554 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2156 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2159 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2152 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2158 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 358 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 357 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1073 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1077 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1069 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1076 |
innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 5787 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 5536 |
final var diff = groundTruthGravityNorm - norm;
return diff * diff;
} catch (final AlgebraException e) {
return Double.MAX_VALUE;
}
}
/**
* Computes a preliminary solution for a subset of samples picked by a robust estimator.
*
* @param samplesIndices indices of samples picked by the robust estimator.
* @param solutions list where estimated preliminary solution will be stored.
*/
protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {
final var meas = new ArrayList<StandardDeviationBodyKinematics>();
for (final var samplesIndex : samplesIndices) {
meas.add(measurements.get(samplesIndex));
}
try {
final var result = new PreliminaryResult();
result.estimatedMa = getInitialMa();
innerCalibrator.setBiasCoordinates(biasX, biasY, biasZ);
innerCalibrator.setInitialMa(result.estimatedMa);
innerCalibrator.setCommonAxisUsed(commonAxisUsed);
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 5886 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 6433 |
estimatedMa = preliminaryResult.estimatedMa;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
}
/**
* Internally sets ground truth gravity norm to be expected at location where
* measurements have been made, expressed in meters per squared second
* (m/s^2).
*
* @param groundTruthGravityNorm ground truth gravity norm or null if
* undefined.
* @throws IllegalArgumentException if provided value is negative.
*/
private void internalSetGroundTruthGravityNorm(final Double groundTruthGravityNorm) {
if (groundTruthGravityNorm != null && groundTruthGravityNorm < 0.0) {
throw new IllegalArgumentException();
}
this.groundTruthGravityNorm = groundTruthGravityNorm;
}
/**
* Converts acceleration value and unit to meters per squared second.
*
* @param value acceleration value.
* @param unit unit of acceleration value.
* @return converted value.
*/
private static double convertAcceleration(final double value, final AccelerationUnit unit) {
return AccelerationConverter.convert(value, unit, AccelerationUnit.METERS_PER_SQUARED_SECOND);
}
/**
* Converts acceleration instance to meters per squared second.
*
* @param acceleration acceleration instance to be converted.
* @return converted value.
*/
private static double convertAcceleration(final Acceleration acceleration) {
return convertAcceleration(acceleration.getValue().doubleValue(), acceleration.getUnit());
}
/**
* Internal class containing estimated preliminary result.
*/
protected static class PreliminaryResult {
/**
* Estimated accelerometer scale factors and cross coupling errors.
* This is the product of matrix Ta containing cross coupling errors and Ka
* containing scaling factors.
* So tat:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Ka = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Ta = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Ma = [sx mxy mxz] = Ta*Ka = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Ma matrix
* becomes upper diagonal:
* <pre>
* Ma = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*/
private Matrix estimatedMa; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 3035 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 5343 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 3122 |
}
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationFrameBodyKinematics>();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
inlierMeasurements.add(measurements.get(i));
}
}
try {
nonLinearCalibrator.setInitialBias(preliminaryResult.estimatedBiases); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3862 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4148 |
jacobian.setElementAt(2, 8, omegatruey);
jacobian.setElementAt(2, 9, 0.0);
jacobian.setElementAt(2, 10, 0.0);
jacobian.setElementAt(2, 11, 0.0);
jacobian.setElementAt(2, 12, 0.0);
jacobian.setElementAt(2, 13, 0.0);
jacobian.setElementAt(2, 14, 0.0);
jacobian.setElementAt(2, 15, ftruex);
jacobian.setElementAt(2, 16, ftruey);
jacobian.setElementAt(2, 17, ftruez);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var sx = result[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4928 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5556 |
jacobian.setElementAt(17, 17, m33);
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Internal method to perform calibration when common z-axis is assumed for both
* the accelerometer and gyroscope and G-dependent cross biases are ignored.
*
* @throws AlgebraException if there are numerical errors.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
* @throws InvalidSourceAndDestinationFrameTypeException never happens.
*/
private void calibrateCommonAxis() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException, InvalidSourceAndDestinationFrameTypeException {
// The gyroscope model is
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// Ωmeas = bg + (I + Mg) * Ωtrue + Gg * ftrue
// Since G-dependent cross biases are ignored, we can assume that Gg = 0
// Hence:
// Ωmeas = bg + (I + Mg) * Ωtrue
// For convergence purposes of the Levenberg-Marquardt algorithm, the
// gyroscope model can be better expressed as:
// Ωmeas = T*K*(Ωtrue + b)
// Ωmeas = M*(Ωtrue + b)
// Ωmeas = M*Ωtrue + M*b
// where:
// M = I + Mg
// bg = M*b = (I + Mg)*b --> b = M^-1*bg
// We know that the norm of the true angular rate when the device is in a pixed
// and unknown position and orientation is equal to the Earth rotation rate.
// ||Ωtrue|| = 7.292115E-5 rad/s
// Hence
// Ωmeas - M*b = M*Ωtrue
// M^-1 * (Ωmeas - M*b) = Ωtrue
// ||Ωtrue||^2 = (M^-1 * (Ωmeas - M*b))^T*(M^-1 * (Ωmeas - M*b))
// ||Ωtrue||^2 = (Ωmeas - M*b)^T * (M^-1)^T * M^-1 * (Ωmeas - M*b)
// ||Ωtrue||^2 = (Ωmeas - M*b)^T * ||M^-1||^2 * (Ωmeas - M*b)
// ||Ωtrue||^2 = ||Ωmeas - M*b||^2 * ||M^-1||^2
// Where:
// b = [bx]
// [by]
// [bz]
// M = [m11 m12 m13]
// [0 m22 m23]
// [0 0 m33]
final var gradientEstimator = new GradientEstimator(this::evaluateCommonAxis);
final var initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
initialM.add(getInitialMg());
// Force initial M to be upper diagonal
initialM.setElementAt(1, 0, 0.0);
initialM.setElementAt(2, 0, 0.0);
initialM.setElementAt(2, 1, 0.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 448 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 437 |
private final EasyGyroscopeCalibrator innerCalibrator = new EasyGyroscopeCalibrator();
/**
* Contains normalized start gravity coordinates.
* This is reused when computing error residuals.
*/
private final InhomogeneousPoint3D startPoint = new InhomogeneousPoint3D();
/**
* Contains estimated normalized end gravity coordinates.
* This is reused when computing error residuals.
*/
private final InhomogeneousPoint3D endPoint = new InhomogeneousPoint3D();
/**
* Contains expected normalized end gravity coordinates.
* This is reused when computing error residuals.
*/
private final InhomogeneousPoint3D expectedEndPoint = new InhomogeneousPoint3D();
/**
* Contains amount of rotation for a given sequence and preliminary
* solution.
* This is reused when computing error residuals.
*/
private final Quaternion q = new Quaternion();
/**
* Array containing measured specific force coordinates.
* This is reused when computing error residuals.
*/
private final double[] measuredSpecificForce = new double[BodyKinematics.COMPONENTS];
/**
* Array containing fixed specific force coordinates.
* This is reused when computing error residuals.
*/
private final double[] fixedSpecificForce = new double[BodyKinematics.COMPONENTS];
/**
* Array containing measured angular rate coordinates.
* This is reused when computing error residuals.
*/
private final double[] measuredAngularRate = new double[BodyKinematics.COMPONENTS];
/**
* Array containing fixed angular rate coordinates.
* This is reused when computing error residuals.
*/
private final double[] fixedAngularRate = new double[BodyKinematics.COMPONENTS];
/**
* An acceleration fixer.
* This is reused when computing error residuals.
*/
private final AccelerationFixer accelerationFixer = new AccelerationFixer();
/**
* An angular rate fixer.
* This is reused when computing error residuals.
*/
private final AngularRateFixer angularRateFixer = new AngularRateFixer();
/**
* Constructor.
*/
protected RobustEasyGyroscopeCalibrator() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4173 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4981 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3683 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4501 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4591 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4027 |
if (estimatedMm == null) {
estimatedMm = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
} else {
estimatedMm.initialize(0.0);
}
estimatedMm.setElementAt(0, 0, sx);
estimatedMm.setElementAt(0, 1, mxy);
estimatedMm.setElementAt(1, 1, sy);
estimatedMm.setElementAt(0, 2, mxz);
estimatedMm.setElementAt(1, 2, myz);
estimatedMm.setElementAt(2, 2, sz);
estimatedCovariance = fitter.getCovar();
// propagate covariance matrix so that all parameters are taken into
// account in the order: bx, by, bz, sx, sy, sz, mxy, mxz, myx,
// myz, mzx, mzy.
// We define a lineal function mapping original parameters for the common
// axis case to the general case
// [bx'] = [1 0 0 0 0 0 0 0 0][bx]
// [by'] [0 1 0 0 0 0 0 0 0][by]
// [bz'] [0 0 1 0 0 0 0 0 0][bz]
// [sx'] [0 0 0 1 0 0 0 0 0][sx]
// [sy'] [0 0 0 0 1 0 0 0 0][sy]
// [sz'] [0 0 0 0 0 1 0 0 0][sz]
// [mxy'] [0 0 0 0 0 0 1 0 0][mxy]
// [mxz'] [0 0 0 0 0 0 0 1 0][mxz]
// [myx'] [0 0 0 0 0 0 0 0 0][myz]
// [myz'] [0 0 0 0 0 0 0 0 1]
// [mzx'] [0 0 0 0 0 0 0 0 0]
// [mzy'] [0 0 0 0 0 0 0 0 0]
// As defined in com.irurueta.statistics.MultivariateNormalDist,
// if we consider the jacobian of the lineal application the matrix shown
// above, then covariance can be propagated as follows
final var jacobian = Matrix.identity(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
jacobian.setElementAt(8, 8, 0.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 7562 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 8091 |
estimatedMm = preliminaryResult.estimatedMm;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
}
/**
* Internally sets ground truth magnetic flux density norm to be expected at location where
* measurements have been made, expressed in Teslas (T).
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
* @throws IllegalArgumentException if provided value is negative.
*/
private void internalSetGroundTruthMagneticFluxDensityNorm(final Double groundTruthMagneticFluxDensityNorm) {
if (groundTruthMagneticFluxDensityNorm != null && groundTruthMagneticFluxDensityNorm < 0.0) {
throw new IllegalArgumentException();
}
this.groundTruthMagneticFluxDensityNorm = groundTruthMagneticFluxDensityNorm;
}
/**
* Converts magnetic flux density value and unit to Teslas.
*
* @param value magnetic flux density value.
* @param unit unit of magnetic flux density value.
* @return converted value.
*/
private static double convertMagneticFluxDensity(final double value, final MagneticFluxDensityUnit unit) {
return MagneticFluxDensityConverter.convert(value, unit, MagneticFluxDensityUnit.TESLA);
}
/**
* Converts magnetic flux density instance to Teslas.
*
* @param magneticFluxDensity magnetic flux density instance to be converted.
* @return converted value.
*/
private static double convertMagneticFluxDensity(final MagneticFluxDensity magneticFluxDensity) {
return convertMagneticFluxDensity(magneticFluxDensity.getValue().doubleValue(), magneticFluxDensity.getUnit());
}
/**
* Internal class containing estimated preliminary result.
*/
protected static class PreliminaryResult {
/**
* Estimated magnetometer soft-iron matrix containing scale factors
* and cross coupling errors.
* This is the product of matrix Tm containing cross coupling errors and Km
* containing scaling factors.
* So tat:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km
* [myx sy myz]
* [mzx mzy sz ]
* </pre>
* Where:
* <pre>
* Km = [sx 0 0 ]
* [0 sy 0 ]
* [0 0 sz]
* </pre>
* and
* <pre>
* Tm = [1 -alphaXy alphaXz ]
* [alphaYx 1 -alphaYz]
* [-alphaZx alphaZy 1 ]
* </pre>
* Hence:
* <pre>
* Mm = [sx mxy mxz] = Tm*Km = [sx -sy * alphaXy sz * alphaXz ]
* [myx sy myz] [sx * alphaYx sy -sz * alphaYz]
* [mzx mzy sz ] [-sx * alphaZx sy * alphaZy sz ]
* </pre>
* This instance allows any 3x3 matrix however, typically alphaYx, alphaZx and alphaZy
* are considered to be zero if the accelerometer z-axis is assumed to be the same
* as the body z-axis. When this is assumed, myx = mzx = mzy = 0 and the Mm matrix
* becomes upper diagonal:
* <pre>
* Mm = [sx mxy mxz]
* [0 sy myz]
* [0 0 sz ]
* </pre>
* Values of this matrix are unit-less.
*/
private Matrix estimatedMm; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 1895 |
| com/irurueta/navigation/inertial/estimators/ECIKinematicsEstimator.java | 1064 |
final Time timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz, final BodyKinematics result) {
estimateKinematics(
TimeConverter.convert(timeInterval.getValue().doubleValue(), timeInterval.getUnit(), TimeUnit.SECOND),
frame.getCoordinateTransformation(), oldC, frame.getVx(), frame.getVy(), frame.getVz(),
oldVx, oldVy, oldVz, frame.getX(), frame.getY(), frame.getZ(), result);
}
/**
* Estimates body kinematics (specific force applied to a body and its angular rates).
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param frame body ECEF frame containing current position, velocity and
* body-to-ECEF frame coordinate transformation.
* @param oldC previous body-to-ECEF-frame coordinate transformation matrix.
* @param oldVelocity previous velocity of body frame with respect ECEF frame.
* @param result instance where body kinematics estimation will be stored.
* @throws IllegalArgumentException if provided time interval is negative or coordinated transformation matrices
* are not ECEF frame valid.
*/
public static void estimateKinematics(
final double timeInterval, final ECEFFrame frame, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 16986 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17231 |
final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
try {
navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldFrame previous NED frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateNED(
final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 16987 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17383 |
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
try {
navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldFrame previous NED frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateNED(
final double timeInterval, final NEDFrame oldFrame, final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4437 |
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 4771 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2505 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2727 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1701 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1820 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1774 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 1197 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 1865 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1821 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 2537 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2429 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2506 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3587 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 2927 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 2533 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1718 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2507 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3546 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 1212 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3598 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3634 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1825 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 1907 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3445 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3407 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1201 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1165 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1987 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1952 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2015 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 1980 |
public void getInitialMa(final Matrix result) {
if (result.getRows() != BodyKinematics.COMPONENTS || result.getColumns() != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result.setElementAtIndex(0, initialSx);
result.setElementAtIndex(1, initialMyx);
result.setElementAtIndex(2, initialMzx);
result.setElementAtIndex(3, initialMxy);
result.setElementAtIndex(4, initialSy);
result.setElementAtIndex(5, initialMzy);
result.setElementAtIndex(6, initialMxz);
result.setElementAtIndex(7, initialMyz);
result.setElementAtIndex(8, initialSz);
}
/**
* Sets initial scale factors and cross coupling errors matrix.
*
* @param initialMa initial scale factors and cross coupling errors matrix.
* @throws IllegalArgumentException if provided matrix is not 3x3.
* @throws LockedException if calibrator is currently running.
*/
@Override
public void setInitialMa(final Matrix initialMa) throws LockedException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4993 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5236 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2372 |
fmeasZ = point[2];
gradientEstimator.gradient(params, derivatives);
return evaluateGeneral(params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var m11 = result[0];
final var m21 = result[1];
final var m31 = result[2];
final var m12 = result[3];
final var m22 = result[4];
final var m32 = result[5];
final var m13 = result[6];
final var m23 = result[7];
final var m33 = result[8];
final var crossCoupling = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6078 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5672 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3215 |
fmeasZ = point[2];
gradientEstimator.gradient(params, derivatives);
return evaluateCommonAxis(params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var bx = result[0];
final var by = result[1];
final var bz = result[2];
final var m11 = result[3];
final var m12 = result[4];
final var m22 = result[5];
final var m13 = result[6];
final var m23 = result[7];
final var m33 = result[8];
final var bias = new Matrix(BodyKinematics.COMPONENTS, 1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 194 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 191 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 3025 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 3112 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 2965 |
result.estimatedMa = nonLinearCalibrator.getEstimatedMa();
if (keepCovariance) {
result.covariance = nonLinearCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = nonLinearCalibrator.getEstimatedMse();
result.estimatedChiSq = nonLinearCalibrator.getEstimatedChiSq();
}
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationFrameBodyKinematics>(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4160 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6315 |
if (estimatedMg == null) {
estimatedMg = m;
} else {
estimatedMg.copyFrom(m);
}
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
estimatedMg.setElementAt(i, i, estimatedMg.getElementAt(i, i) - 1.0);
}
if (estimatedGg == null) {
estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedGg.initialize(0.0);
}
estimatedCovariance = fitter.getCovar();
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Computes gravity versor error at the end of a sequence using provided
* parameters.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param i row position.
* @param params array containing parameters for the general purpose case
* when G-dependent cross biases are taken into account. Must
* have length 18.
* @return error between estimated and measured gravity versor.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 712 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 193 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates gyroscope calibration parameters containing bias, scale factors
* cross-coupling errors and g-dependant cross biases.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 910 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 904 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 908 |
super(position, measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates magnetometer calibration parameters containing soft-iron
* scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 886 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 879 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 884 |
super(position, measurements, commonAxisUsed, hardIron, initialMm, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates magnetometer calibration parameters containing soft-iron
* scale factors and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9443 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9482 |
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final ECEFVelocity oldVelocity, final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition, oldC,
oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
convertAngularSpeedToDouble(angularRateX),
convertAngularSpeedToDouble(angularRateY),
convertAngularSpeedToDouble(angularRateZ), result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 63 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 66 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 70 |
QualityScoredAccelerometerCalibrator {
/**
* Indicates whether by default a common z-axis is assumed for both the accelerometer
* and gyroscope.
*/
public static final boolean DEFAULT_USE_COMMON_Z_AXIS = false;
/**
* Required minimum number of measurements.
*/
public static final int MINIMUM_MEASUREMENTS = 4;
/**
* Indicates that by default a linear calibrator is used for preliminary solution estimation.
* The result obtained on each preliminary solution might be later refined.
*/
public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;
/**
* Indicates that by default preliminary solutions are refined.
*/
public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default using a non-linear calibrator
* (which uses a Levenberg-Marquardt fitter).
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Contains a list of body kinematics measurements taken at different
* frames (positions, orientations and velocities) and containing the standard
* deviations of accelerometer and gyroscope measurements.
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*/
protected List<StandardDeviationFrameBodyKinematics> measurements; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2706 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2931 |
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasY known y coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasZ known z coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3532 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3815 |
final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
listener);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasY known y coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasZ known z coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 5821 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 5571 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 6365 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 6088 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 10518 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 11020 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 7495 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 8021 |
result.estimatedMa = innerCalibrator.getEstimatedMa();
if (keepCovariance) {
result.covariance = innerCalibrator.getEstimatedCovariance();
} else {
result.covariance = null;
}
result.estimatedMse = innerCalibrator.getEstimatedMse();
result.estimatedChiSq = innerCalibrator.getEstimatedChiSq();
solutions.add(result);
} catch (final LockedException | CalibrationException | NotReadyException e) {
solutions.clear();
}
}
/**
* Attempts to refine calibration parameters if refinement is requested.
* This method returns a refined solution or provided input if refinement is not
* requested or has failed.
* If refinement is enabled and it is requested to keep covariance, this method
* will also keep covariance of refined position.
*
* @param preliminaryResult a preliminary result.
*/
protected void attemptRefine(final PreliminaryResult preliminaryResult) {
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = measurements.size();
final var inlierMeasurements = new ArrayList<StandardDeviationBodyKinematics>(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4138 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4541 |
jacobian.setElementAt(1, 17, 0.0);
jacobian.setElementAt(2, 0, 0.0);
jacobian.setElementAt(2, 1, 0.0);
jacobian.setElementAt(2, 2, 1.0);
jacobian.setElementAt(2, 3, 0.0);
jacobian.setElementAt(2, 4, 0.0);
jacobian.setElementAt(2, 5, omegatruez);
jacobian.setElementAt(2, 6, 0.0);
jacobian.setElementAt(2, 7, 0.0);
jacobian.setElementAt(2, 8, 0.0);
jacobian.setElementAt(2, 9, 0.0);
jacobian.setElementAt(2, 10, 0.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3999 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4783 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4086 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4882 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2771 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2991 |
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
commonAxisUsed);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3584 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3871 |
final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
biasX, biasY, biasZ, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
biasX, biasY, biasZ, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
biasX, biasY, biasZ, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, biasX, biasY, biasZ, listener);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, biasX, biasY, biasZ, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 6 samples.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3509 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4301 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustKnownBiasEasyGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3596 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4402 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg,
final RobustKnownBiasEasyGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 3920 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 3970 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
convertAccelerationToDouble(fx), convertAccelerationToDouble(fy), convertAccelerationToDouble(fz),
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECI(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java | 1885 |
| com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java | 1865 |
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java | 1616 |
gyroBiasZ = getValueOrZero(state.getGyroBiasZ());
} else {
accelBiasX = 0.0;
accelBiasY = 0.0;
accelBiasZ = 0.0;
gyroBiasX = 0.0;
gyroBiasY = 0.0;
gyroBiasZ = 0.0;
}
final var fx = kinematics.getFx();
final var fy = kinematics.getFy();
final var fz = kinematics.getFz();
final var angularRateX = kinematics.getAngularRateX();
final var angularRateY = kinematics.getAngularRateY();
final var angularRateZ = kinematics.getAngularRateZ();
correctedKinematics.setSpecificForceCoordinates(fx - accelBiasX, fy - accelBiasY, fz - accelBiasZ);
correctedKinematics.setAngularRateCoordinates(
angularRateX - gyroBiasX,
angularRateY - gyroBiasY,
angularRateZ - gyroBiasZ);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3222 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3708 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4152 |
return initial;
}
@Override
public void evaluate(
final int i, final double[] point, final double[] result,
final double[] params, final Matrix jacobian) {
// We know that:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + myx * ftruex + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + mzx * ftruex + mzy * ftruey + ftruez + sz * ftruez
// Hence, the derivatives respect the parameters sx, sy, sz,
// mxy, mxz, myx, myz, mzx and mzy is:
// d(fmeasx)/d(sx) = ftruex
// d(fmeasx)/d(sy) = 0.0
// d(fmeasx)/d(sz) = 0.0
// d(fmeasx)/d(mxy) = ftruey
// d(fmeasx)/d(mxz) = ftruez
// d(fmeasx)/d(myx) = 0.0
// d(fmeasx)/d(myz) = 0.0
// d(fmeasx)/d(mzx) = 0.0
// d(fmeasx)/d(mzy) = 0.0
// d(fmeasy)/d(sx) = 0.0
// d(fmeasy)/d(sy) = ftruey
// d(fmeasy)/d(sz) = 0.0
// d(fmeasy)/d(mxy) = 0.0
// d(fmeasy)/d(mxz) = 0.0
// d(fmeasy)/d(myx) = ftruex
// d(fmeasy)/d(myz) = ftruez
// d(fmeasy)/d(mzx) = 0.0
// d(fmeasy)/d(mzy) = 0.0
// d(fmeasz)/d(sx) = 0.0
// d(fmeasz)/d(sy) = 0.0
// d(fmeasz)/d(sz) = ftruez
// d(fmeasz)/d(mxy) = 0.0
// d(fmeasz)/d(mxz) = 0.0
// d(fmeasz)/d(myx) = 0.0
// d(fmeasz)/d(myz) = 0.0
// d(fmeasz)/d(mzx) = ftruex
// d(fmeasz)/d(mzy) = ftruey
final var sx = params[0];
final var sy = params[1];
final var sz = params[2];
final var mxy = params[3];
final var mxz = params[4];
final var myx = params[5];
final var myz = params[6];
final var mzx = params[7];
final var mzy = params[8];
final var ftruex = point[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3722 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3993 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4468 |
return initial;
}
@Override
public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
final Matrix jacobian) {
// We know that:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + ftruez + sz * ftruez
// Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
// sz, mxy, mxz, myz
// d(fmeasx)/d(bx) = 1.0
// d(fmeasx)/d(by) = 0.0
// d(fmeasx)/d(bz) = 0.0
// d(fmeasx)/d(sx) = ftruex
// d(fmeasx)/d(sy) = 0.0
// d(fmeasx)/d(sz) = 0.0
// d(fmeasx)/d(mxy) = ftruey
// d(fmeasx)/d(mxz) = ftruez
// d(fmeasx)/d(myz) = 0.0
// d(fmeasy)/d(bx) = 0.0
// d(fmeasy)/d(by) = 1.0
// d(fmeasy)/d(bz) = 0.0
// d(fmeasy)/d(sx) = 0.0
// d(fmeasy)/d(sy) = ftruey
// d(fmeasy)/d(sz) = 0.0
// d(fmeasy)/d(mxy) = 0.0
// d(fmeasy)/d(mxz) = 0.0
// d(fmeasy)/d(myz) = ftruez
// d(fmeasz)/d(bx) = 0.0
// d(fmeasz)/d(by) = 0.0
// d(fmeasz)/d(bz) = 1.0
// d(fmeasz)/d(sx) = 0.0
// d(fmeasz)/d(sy) = 0.0
// d(fmeasz)/d(sz) = ftruez
// d(fmeasz)/d(mxy) = 0.0
// d(fmeasz)/d(mxz) = 0.0
// d(fmeasz)/d(myz) = 0.0
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var sx = params[3];
final var sy = params[4];
final var sz = params[5];
final var mxy = params[6];
final var mxz = params[7];
final var myz = params[8];
final var ftruex = point[0]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 708 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 818 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 189 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 826 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java | 602 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 712 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 585 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 194 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 191 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 193 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 906 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 910 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 904 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 908 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 794 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 164 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 801 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java | 576 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 687 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java | 561 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 170 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 167 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 886 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 5865 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 5615 |
innerCalibrator.setGroundTruthGravityNorm(groundTruthGravityNorm);
innerCalibrator.setMeasurements(inlierMeasurements);
innerCalibrator.calibrate();
estimatedMa = innerCalibrator.getEstimatedMa();
estimatedMse = innerCalibrator.getEstimatedMse();
estimatedChiSq = innerCalibrator.getEstimatedChiSq();
if (keepCovariance) {
estimatedCovariance = innerCalibrator.getEstimatedCovariance();
} else {
estimatedCovariance = null;
}
} catch (final LockedException | CalibrationException | NotReadyException e) {
estimatedCovariance = preliminaryResult.covariance;
estimatedMa = preliminaryResult.estimatedMa;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
} else {
estimatedCovariance = preliminaryResult.covariance;
estimatedMa = preliminaryResult.estimatedMa;
estimatedMse = preliminaryResult.estimatedMse;
estimatedChiSq = preliminaryResult.estimatedChiSq;
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4909 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 5001 |
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params)
throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m21 = params[4];
final var m31 = params[5];
final var m12 = params[6];
final var m22 = params[7];
final var m32 = params[8];
final var m13 = params[9];
final var m23 = params[10];
final var m33 = params[11]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 955 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 1099 |
a.setElementAt(i, 17, fTrueZ);
b.setElementAtIndex(i, omegaMeasZ - omegaTrueZ);
i++;
}
final var unknowns = Utils.solve(a, b);
final var bx = unknowns.getElementAtIndex(0);
final var by = unknowns.getElementAtIndex(1);
final var bz = unknowns.getElementAtIndex(2);
final var sx = unknowns.getElementAtIndex(3);
final var sy = unknowns.getElementAtIndex(4);
final var sz = unknowns.getElementAtIndex(5);
final var mxy = unknowns.getElementAtIndex(6);
final var mxz = unknowns.getElementAtIndex(7);
final var myz = unknowns.getElementAtIndex(8); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java | 745 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 730 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java | 695 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java | 682 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java | 1415 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 1385 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1433 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1404 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 769 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 755 |
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.LMEDS; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4574 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6688 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5610 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7816 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5109 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7179 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6126 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8287 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 337 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 336 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1055 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1054 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 289 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 288 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1005 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1006 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 540 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 538 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2149 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2145 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 558 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 558 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2163 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2162 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 362 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 361 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1081 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1080 |
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | IOException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.LMEDS; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 7276 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 7307 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final ECEFVelocity oldVelocity, final BodyKinematics kinematics,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(),
kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
kinematics.getAngularRateX(), kinematics.getAngularRateY(), kinematics.getAngularRateZ(), result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param kinematics body kinematics containing specific forces and angular rates applied to
* the body.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2786 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2921 |
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3338 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3469 |
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2720 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2841 |
final double[] bias, final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3229 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3351 |
final double[] bias, final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3315 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3451 |
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3866 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3998 |
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3245 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3368 |
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3749 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3872 |
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4875 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5566 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6315 |
if (estimatedMg == null) {
estimatedMg = m;
} else {
estimatedMg.copyFrom(m);
}
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
estimatedMg.setElementAt(i, i, estimatedMg.getElementAt(i, i) - 1.0);
}
if (estimatedGg == null) {
estimatedGg = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
} else {
estimatedGg.initialize(0.0);
}
estimatedCovariance = fitter.getCovar();
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Computes gravity versor error at the end of a sequence using provided
* parameters.
* This method is internally executed during gradient estimation and
* Levenberg-Marquardt fitting needed for calibration computation.
*
* @param i row position.
* @param params array containing parameters for the general purpose case
* when G-dependent cross biases are taken into account. Must
* have length 18.
* @return error between estimated and measured gravity versor.
* @throws EvaluationException if there are numerical instabilities.
*/
private double evaluateGeneralWithGDependentCrossBiases(final int i, final double[] params) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5006 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6043 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7308 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8436 |
final boolean estimateGDependentCrossBiases, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5143 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6180 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7161 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8289 |
final boolean estimateGDependentCrossBiases, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustKnownBiasTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5527 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6544 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7784 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8894 |
final boolean estimateGDependentCrossBiases, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5661 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6678 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7640 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8748 |
final boolean estimateGDependentCrossBiases, final double[] initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 3553 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 3694 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron array does not have length 3.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] hardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3052 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3193 |
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3637 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3778 |
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4064 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4205 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron array does not have length 3.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] initialHardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3560 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3701 |
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4139 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4280 |
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSGNSSLooselyCoupledKalmanFilteredEstimator.java | 1611 |
| com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java | 1562 |
if (lastStateTimestamp != null && timestamp - lastStateTimestamp <= insEstimator.getEpochInterval()) {
return false;
}
try {
running = true;
if (listener != null) {
listener.onUpdateGNSSMeasurementsStart(this);
}
this.measurements = new ArrayList<>(measurements);
lsEstimator.setMeasurements(this.measurements);
lsEstimator.setPriorPositionAndVelocityFromEstimation(estimation);
if (estimation != null) {
lsEstimator.estimate(estimation);
} else {
estimation = lsEstimator.estimate();
}
if (listener != null) {
listener.onUpdateGNSSMeasurementsEnd(this);
}
} catch (final GNSSException e) {
throw new INSGNSSException(e);
} finally {
running = false;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 1334 |
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 1419 |
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 1841 |
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 1906 |
public void fix(
final double measuredAngularRateX, final double measuredAngularRateY, final double measuredAngularRateZ,
final double trueFx, final double trueFy, final double trueFz,
final double biasX, final double biasY, final double biasZ,
final double sx, final double sy, final double sz,
final double mxy, final double mxz, final double myx,
final double myz, final double mzx, final double mzy,
final double g11, final double g21, final double g31,
final double g12, final double g22, final double g32,
final double g13, final double g23, final double g33, final double[] result) throws AlgebraException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 5165 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2545 |
setResult(crossCoupling);
// taking into account that:
// Ma = [sx mxy mxz] = [m11 m12 m13]
// [myx sy myz] [m21 m22 m23]
// [mzx mzy sz ] [m31 m32 m33]
// propagate covariance so that all parameters are taken into account
// in the order: sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy
// We define a lineal function mapping original parameters for the
// common axis case to the general case
// [sx'] = [1 0 0 0 0 0][sx ]
// [sy'] [0 0 1 0 0 0][mxy]
// [sz'] [0 0 0 0 0 1][sy ]
// [mxy'] [0 1 0 0 0 0][mxz]
// [mxz'] [0 0 0 1 0 0][myz]
// [myx'] [0 0 0 0 0 0][sz ]
// [myz'] [0 0 0 0 1 0]
// [mzx'] [0 0 0 0 0 0]
// [mzy'] [0 0 0 0 0 0]
// As defined in com.irurueta.statistics.MultivariateNormalDist,
// if we consider the jacobian of the lineal application the matrix shown
// above, then covariance can be propagated as follows
final var jacobian = new Matrix(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
jacobian.setElementAt(0, 0, 1.0);
jacobian.setElementAt(1, 2, 1.0);
jacobian.setElementAt(2, 5, 1.0);
jacobian.setElementAt(3, 1, 1.0);
jacobian.setElementAt(4, 3, 1.0);
jacobian.setElementAt(6, 4, 1.0);
// propagated covariance is J * Cov * J'
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
}
/**
* Makes proper conversion of internal cross-coupling and bias matrices.
*
* @param m internal cross-coupling matrix.
*/
private void setResult(final Matrix m) {
// Because:
// M = I + Ma
// Then:
// Ma = M - I
if (estimatedMa == null) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6266 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 5001 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6438 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3403 |
private double evaluateGeneral(final double[] params) throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m21 = params[4];
final var m31 = params[5];
final var m12 = params[6];
final var m22 = params[7];
final var m32 = params[8];
final var m13 = params[9];
final var m23 = params[10];
final var m33 = params[11];
return evaluate(bx, by, bz, m11, m21, m31, m12, m22, m32, m13, m23, m33); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1510 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1550 |
b.setElementAtIndex(i, fMeasZ - fTrueZ - biasZ);
i++;
}
final var unknowns = Utils.solve(a, b);
final var sx = unknowns.getElementAtIndex(0);
final var sy = unknowns.getElementAtIndex(1);
final var sz = unknowns.getElementAtIndex(2);
final var mxy = unknowns.getElementAtIndex(3);
final var mxz = unknowns.getElementAtIndex(4);
final var myx = unknowns.getElementAtIndex(5);
final var myz = unknowns.getElementAtIndex(6);
final var mzx = unknowns.getElementAtIndex(7);
final var mzy = unknowns.getElementAtIndex(8); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java | 2689 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java | 2973 |
final double biasZ, final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx, final double initialMyz,
final double initialMzx, final double initialMzy,
final KnownBiasAndPositionAccelerometerCalibrationListener listener) {
this(commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
initialMyz, initialMzx, initialMzy, listener);
try {
setPosition(position);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param biasX x-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param biasY y-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param biasZ z-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
*/
public KnownBiasAndPositionAccelerometerCalibrator(
final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 911 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 1022 |
a.setElementAt(i, 5, fTrueZ);
b.setElementAtIndex(i, fMeasZ - fTrueZ);
i++;
}
final var unknowns = Utils.solve(a, b);
final var bx = unknowns.getElementAtIndex(0);
final var by = unknowns.getElementAtIndex(1);
final var bz = unknowns.getElementAtIndex(2);
final var sx = unknowns.getElementAtIndex(3);
final var sy = unknowns.getElementAtIndex(4);
final var sz = unknowns.getElementAtIndex(5);
final var mxy = unknowns.getElementAtIndex(6);
final var mxz = unknowns.getElementAtIndex(7);
final var myz = unknowns.getElementAtIndex(8); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3950 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4346 |
return BodyKinematics.COMPONENTS;
}
@Override
public int getNumberOfVariables() {
// The multivariate function returns the components of measured specific force
return BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[GENERAL_UNKNOWNS];
initial[0] = initialBiasX;
initial[1] = initialBiasY;
initial[2] = initialBiasZ;
initial[3] = initialSx;
initial[4] = initialSy;
initial[5] = initialSz;
initial[6] = initialMxy;
initial[7] = initialMxz;
initial[8] = initialMyx;
initial[9] = initialMyz;
initial[10] = initialMzx;
initial[11] = initialMzy; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java | 2971 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java | 3283 |
final double initialBiasX, final double initialBiasY, final double initialBiasZ,
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy,
final KnownPositionAccelerometerCalibratorListener listener) {
this(commonAxisUsed, initialBiasX, initialBiasY, initialBiasZ, initialSx, initialSy, initialSz,
initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy, listener);
try {
setPosition(position);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBiasX initial x-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialBiasY initial y-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialBiasZ initial z-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
*/
public KnownPositionAccelerometerCalibrator(
final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double initialBiasX, final double initialBiasY, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 848 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 961 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 908 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 332 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 969 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java | 915 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 798 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 913 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 861 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 283 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 920 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java | 867 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1699 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1868 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1771 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 533 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1888 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1790 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1721 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1886 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1790 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 552 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1906 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1812 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 874 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 986 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 933 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 357 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 995 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 942 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 855 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 1788 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 337 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java | 1821 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 809 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1740 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 289 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1772 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1710 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3592 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 537 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3657 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1727 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3612 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 556 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3675 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 880 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1813 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 362 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1845 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1052 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1048 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1007 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 999 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2141 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2138 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2160 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2156 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1077 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1073 |
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) {
throw new CalibrationException(e);
} finally {
running = false;
}
}
/**
* Returns method being used for robust estimation.
*
* @return method being used for robust estimation.
*/
@Override
public RobustEstimatorMethod getMethod() {
return RobustEstimatorMethod.LMEDS; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3727 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3796 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
default -> new PROMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a solution.
* This must be 3x1 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3221 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3295 |
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg);
default -> new PROMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param sequences collection of sequences containing timestamped body
* kinematics measurements.
* @param initialBias initial gyroscope bias to be used to find a solution.
* This must be 3x1 and is expressed in radians per
* second (rad/s).
* @param initialMg initial gyroscope scale factors and cross coupling
* errors matrix. Must be 3x3.
* @param initialGg initial gyroscope G-dependent cross biases
* introduced on the gyroscope by the specific forces
* sensed by the accelerometer. Must be 3x3.
* @param listener listener to handle events raised by this
* calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if any of the provided values does
* not have proper size.
*/
public static RobustKnownBiasEasyGyroscopeCalibrator create(
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4471 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6571 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4574 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7816 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5507 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7699 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5610 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6688 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5004 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7065 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5109 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8287 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6021 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8171 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6126 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7179 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 997 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1137 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2080 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2222 |
private void calibrateCommonAxis() throws AlgebraException, IOException {
// The magnetometer model is:
// mBmeas = bm + (I + Mm) * mBtrue + w
// Ideally a least squares solution tries to minimize noise component, so:
// mBmeas = bm + (I + Mm) * mBtrue
// Hence:
// [mBmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [mBtruex]
// [mBmeasy] = [by] [0 1 0] [myx sy myz] [mBtruey]
// [mBmeasz] = [bz] [0 0 1] [mzx mzy sz ] [mBtruez]
// where myx = mzx = mzy = 0
// Hence:
// [mBmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [mBtruex]
// [mBmeasy] = [by] [0 1 0] [0 sy myz] [mBtruey]
// [mBmeasz] = [bz] [0 0 1] [0 0 sz ] [mBtruez]
// [mBmeasx] = [bx] + [1+sx mxy mxz ][mBtruex]
// [mBmeasy] [by] [0 1+sy myz ][mBtruey]
// [mBmeasz] [bz] [0 0 1+sz][mBtruez]
// mBmeasx = bx + (1+sx) * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy = by + (1+sy) * mBtruey + myz * mBtruez
// mBmeasz = bz + (1+sz) * mBtruez
// Where the unknowns are: bx, by, bz, sx, sy, sz, mxy mxz, myz
// Reordering:
// mBmeasx = bx + mBtruex + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy = by + mBtruey + sy * mBtruey + myz * mBtruez
// mBmeasz = bz + mBtruez + sz * mBtruez
// mBmeasx - mBtruex = bx + sx * mBtruex + mxy * mBtruey + mxz * mBtruez
// mBmeasy - mBtruey = by + sy * mBtruey + myz * mBtruez
// mBmeasz - mBtruez = bz + sz * mBtruez
// [1 0 0 mBtruex 0 0 mBtruey mBtruez 0 ][bx ] = [mBmeasx - mBtruex]
// [0 1 0 0 mBtruey 0 0 0 mBtruez][by ] [mBmeasy - mBtruey]
// [0 0 1 0 0 mBtruez 0 0 0 ][bz ] [mBmeasz - mBtruez]
// [sx ]
// [sy ]
// [sz ]
// [mxy]
// [mxz]
// [myz]
final WMMEarthMagneticFluxDensityEstimator wmmEstimator;
if (magneticModel != null) {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator(magneticModel);
} else {
wmmEstimator = new WMMEarthMagneticFluxDensityEstimator();
}
final var expectedMagneticFluxDensity = new BodyMagneticFluxDensity();
final var nedFrame = new NEDFrame();
final var earthB = new NEDMagneticFluxDensity();
final var cbn = new CoordinateTransformation(FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
final var cnb = new CoordinateTransformation(FrameType.LOCAL_NAVIGATION_FRAME, FrameType.BODY_FRAME);
final var rows = EQUATIONS_PER_MEASUREMENT * measurements.size();
final var a = new Matrix(rows, COMMON_Z_AXIS_UNKNOWNS); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1109 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1248 |
a.setElementAt(i, 5, bTrueZ);
b.setElementAtIndex(i, bMeasZ - bTrueZ);
i++;
}
final var unknowns = Utils.solve(a, b);
final var bx = unknowns.getElementAtIndex(0);
final var by = unknowns.getElementAtIndex(1);
final var bz = unknowns.getElementAtIndex(2);
final var sx = unknowns.getElementAtIndex(3);
final var sy = unknowns.getElementAtIndex(4);
final var sz = unknowns.getElementAtIndex(5);
final var mxy = unknowns.getElementAtIndex(6);
final var mxz = unknowns.getElementAtIndex(7);
final var myz = unknowns.getElementAtIndex(8); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 7456 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 7980 |
tmp3.setElementAtIndex(2, bMeasZ - hardIronZ);
tmp2.multiply(tmp3, tmp4);
final var norm = Utils.normF(tmp4);
final var diff = groundTruthMagneticFluxDensityNorm - norm;
return diff * diff;
} catch (final AlgebraException e) {
return Double.MAX_VALUE;
}
}
/**
* Computes a preliminary solution for a subset of samples picked by a robust estimator.
*
* @param samplesIndices indices of samples picked by the robust estimator.
* @param solutions list where estimated preliminary solution will be stored.
*/
protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {
final var meas = new ArrayList<StandardDeviationBodyMagneticFluxDensity>();
for (final var samplesIndex : samplesIndices) {
meas.add(this.measurements.get(samplesIndex));
}
try {
final var result = new PreliminaryResult();
result.estimatedMm = getInitialMm(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 10209 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 10263 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11143 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11197 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11481 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11535 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11816 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11867 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12182 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12233 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12551 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12605 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12653 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12701 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12995 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 13043 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4822 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4876 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5164 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5218 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5344 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5398 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5521 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5572 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5623 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5674 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5728 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5782 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5830 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5878 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5926 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5974 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanState.java | 1861 |
| com/irurueta/navigation/inertial/INSTightlyCoupledKalmanState.java | 2103 |
public void copyFrom(final INSLooselyCoupledKalmanState input) {
// copy coordinate transformation matrix
if (input.bodyToEcefCoordinateTransformationMatrix == null) {
bodyToEcefCoordinateTransformationMatrix = null;
} else {
if (bodyToEcefCoordinateTransformationMatrix == null) {
bodyToEcefCoordinateTransformationMatrix = new Matrix(input.bodyToEcefCoordinateTransformationMatrix);
} else {
bodyToEcefCoordinateTransformationMatrix.copyFrom(input.bodyToEcefCoordinateTransformationMatrix);
}
}
vx = input.vx;
vy = input.vy;
vz = input.vz;
x = input.x;
y = input.y;
z = input.z;
accelerationBiasX = input.accelerationBiasX;
accelerationBiasY = input.accelerationBiasY;
accelerationBiasZ = input.accelerationBiasZ;
gyroBiasX = input.gyroBiasX;
gyroBiasY = input.gyroBiasY;
gyroBiasZ = input.gyroBiasZ; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 894 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 841 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 265 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 902 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java | 848 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 788 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 1721 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 270 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java | 1754 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 267 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 982 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 980 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 984 |
final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<PreliminaryResult>() {
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 730 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 803 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 217 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 934 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 288 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1008 |
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 842 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 790 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 212 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 849 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java | 796 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 913 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 860 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 284 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 922 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 869 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 738 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1669 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 218 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1701 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 807 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1740 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 289 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1772 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 215 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 933 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 927 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 932 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 286 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1001 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 999 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1004 |
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3567 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3847 |
final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
biasX, biasY, biasZ);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements,
biasX, biasY, biasZ);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasY known y coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasZ known z coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2785 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3337 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final double[] bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2820 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3371 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2719 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3228 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final double[] bias, final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2752 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3261 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and
* is expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3314 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3865 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final double[] initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3350 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3900 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3244 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3748 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final double[] initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3278 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3782 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java | 624 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java | 609 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 696 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 680 |
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<PreliminaryResult>() {
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return sequences.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(sequences.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustEasyGyroscopeCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3622 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3903 |
final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, biasX, biasY, biasZ);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, biasX, biasY, biasZ);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, biasX, biasY, biasZ);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 6 samples.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5219 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6256 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7554 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8683 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5373 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6410 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7389 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8517 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, commonAxisUsed,
estimateGDependentCrossBiases, bias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5738 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6755 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8029 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 9139 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5890 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6907 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7866 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8976 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3051 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3636 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final double[] hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3087 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3672 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3559 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4138 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final double[] initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3595 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4174 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9865 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9982 |
final double timeInterval, final ECEFFrame oldFrame, final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException {
try {
navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldFrame previous ECEF frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFFrame oldFrame, final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9866 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 10055 |
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException {
try {
navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldFrame previous ECEF frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFFrame oldFrame, final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 9983 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 10124 |
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException {
try {
navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldFrame previous ECEF frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFFrame oldFrame,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 10054 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 10123 |
final double timeInterval, final ECEFFrame oldFrame, final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException {
try {
navigateECEF(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldFrame previous ECEF frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFFrame oldFrame, final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4475 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4592 |
final double timeInterval, final ECIFrame oldFrame, final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
throws InertialNavigatorException {
try {
navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldFrame previous ECI frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateECI(
final Time timeInterval, final ECIFrame oldFrame, final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4476 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4667 |
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
throws InertialNavigatorException {
try {
navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldFrame previous ECI frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateECI(
final Time timeInterval, final ECIFrame oldFrame, final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4593 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4737 |
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
throws InertialNavigatorException {
try {
navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
oldFrame.getCoordinateTransformation(),
oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(), fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldFrame previous ECI frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateECI(
final Time timeInterval, final ECIFrame oldFrame,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4666 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4736 |
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECIFrame result) throws InertialNavigatorException {
try {
navigateECI(timeInterval, oldFrame.getX(), oldFrame.getY(), oldFrame.getZ(),
oldFrame.getCoordinateTransformation(), oldFrame.getVx(), oldFrame.getVy(), oldFrame.getVz(),
fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldFrame previous ECI frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateECI(
final Time timeInterval, final ECIFrame oldFrame,
final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 108 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 212 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 1394 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 1498 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 2052 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 2156 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 2731 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 2829 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 3471 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 3569 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4214 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4318 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4416 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4508 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4600 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4692 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 5293 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 5385 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 5657 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 5749 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java | 1450 |
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java | 1299 |
public boolean getState(final INSTightlyCoupledKalmanState result) {
if (state != null) {
result.copyFrom(state);
return true;
} else {
return false;
}
}
/**
* Gets timestamp expressed in seconds since epoch time when Kalman filter state
* was last propagated.
*
* @return timestamp expressed in seconds since epoch time when Kalman filter
* state was last propagated.
*/
public Double getLastStateTimestamp() {
return lastStateTimestamp;
}
/**
* Gets timestamp since epoch time when Kalman filter state was last propagated.
*
* @param result instance where timestamp since epoch time when Kalman filter
* state was last propagated will be stored.
* @return true if result instance is updated, false otherwise.
*/
public boolean getLastStateTimestampAsTime(final Time result) {
if (lastStateTimestamp != null) {
result.setValue(lastStateTimestamp);
result.setUnit(TimeUnit.SECOND);
return true;
} else {
return false;
}
}
/**
* Gets timestamp since epoch time when Kalman filter state was last propagated.
*
* @return timestamp since epoch time when Kalman filter state was last
* propagated.
*/
public Time getLastStateTimestampAsTime() {
return lastStateTimestamp != null ? new Time(lastStateTimestamp, TimeUnit.SECOND) : null;
}
/**
* Indicates whether this estimator is running or not.
*
* @return true if this estimator is running, false otherwise.
*/
public boolean isRunning() {
return running;
}
/**
* Indicates whether provided measurements are ready to
* be used for an update.
*
* @param measurements measurements to be checked.
* @return true if estimator is ready, false otherwise.
*/
public static boolean isUpdateMeasurementsReady(final Collection<GNSSMeasurement> measurements) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanConfig.java | 133 |
| com/irurueta/navigation/inertial/INSTightlyCoupledKalmanConfig.java | 157 |
public INSLooselyCoupledKalmanConfig(final INSLooselyCoupledKalmanConfig input) {
copyFrom(input);
}
/**
* Gets gyro noise PSD (Power Spectral Density) expressed in squared radians per
* second (rad^2/s).
*
* @return gyro noise PSD.
*/
public double getGyroNoisePSD() {
return gyroNoisePSD;
}
/**
* Sets gyro noise PSD (Power Spectral Density) expressed in squared radians per
* second (rad^2/s).
*
* @param gyroNoisePSD gyro noise PSD.
*/
public void setGyroNoisePSD(final double gyroNoisePSD) {
this.gyroNoisePSD = gyroNoisePSD;
}
/**
* Gets accelerometer noise PSD (Power Spectral Density) expressed in (m^2 * s^-3).
*
* @return accelerometer noise PSD.
*/
public double getAccelerometerNoisePSD() {
return accelerometerNoisePSD;
}
/**
* Sets accelerometer noise PSD (Power Spectral Density) expressed in (m^2 * s^-3).
*
* @param accelerometerNoisePSD accelerometer noise PSD.
*/
public void setAccelerometerNoisePSD(final double accelerometerNoisePSD) {
this.accelerometerNoisePSD = accelerometerNoisePSD;
}
/**
* Gets accelerometer bias random walk PSD (Power Spectral Density) expressed
* in (m^2 * s^-5).
*
* @return accelerometer bias random walk PSD.
*/
public double getAccelerometerBiasPSD() {
return accelerometerBiasPSD;
}
/**
* Sets accelerometer bias random walk PSD (Power Spectral Density) expressed
* in (m^2 * s^-5).
*
* @param accelerometerBiasPSD accelerometer bias random walk PSD.
*/
public void setAccelerometerBiasPSD(final double accelerometerBiasPSD) {
this.accelerometerBiasPSD = accelerometerBiasPSD;
}
/**
* Gets gyro bias random walk PSD (Power Spectral Density) expressed in
* (rad^2 * s^-3).
*
* @return gyro bias random walk PSD.
*/
public double getGyroBiasPSD() {
return gyroBiasPSD;
}
/**
* Sets gyro bias random walk PSD (Power Spectral Density) expressed in
* (rad^2 * s^-3).
*
* @param gyroBiasPSD gyro bias random walk PSD.
*/
public void setGyroBiasPSD(final double gyroBiasPSD) {
this.gyroBiasPSD = gyroBiasPSD;
}
/**
* Gets position measurement noise SD (Standard Deviation) per axis expressed
* in meters (m).
*
* @return position measurement noise SD.
*/
public double getPositionNoiseSD() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6266 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6349 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6438 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3403 |
private double evaluateGeneral(final double[] params) throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m21 = params[4];
final var m31 = params[5];
final var m12 = params[6];
final var m22 = params[7];
final var m32 = params[8];
final var m13 = params[9];
final var m23 = params[10];
final var m33 = params[11]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerLinearLeastSquaresCalibrator.java | 1510 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeLinearLeastSquaresCalibrator.java | 1550 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 2347 |
b.setElementAtIndex(i, fMeasZ - fTrueZ - biasZ);
i++;
}
final var unknowns = Utils.solve(a, b);
final var sx = unknowns.getElementAtIndex(0);
final var sy = unknowns.getElementAtIndex(1);
final var sz = unknowns.getElementAtIndex(2);
final var mxy = unknowns.getElementAtIndex(3);
final var mxz = unknowns.getElementAtIndex(4);
final var myx = unknowns.getElementAtIndex(5);
final var myz = unknowns.getElementAtIndex(6);
final var mzx = unknowns.getElementAtIndex(7);
final var mzy = unknowns.getElementAtIndex(8); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerLinearLeastSquaresCalibrator.java | 913 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeLinearLeastSquaresCalibrator.java | 957 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 1111 |
b.setElementAtIndex(i, fMeasZ - fTrueZ);
i++;
}
final var unknowns = Utils.solve(a, b);
final var bx = unknowns.getElementAtIndex(0);
final var by = unknowns.getElementAtIndex(1);
final var bz = unknowns.getElementAtIndex(2);
final var sx = unknowns.getElementAtIndex(3);
final var sy = unknowns.getElementAtIndex(4);
final var sz = unknowns.getElementAtIndex(5);
final var mxy = unknowns.getElementAtIndex(6);
final var mxz = unknowns.getElementAtIndex(7);
final var myz = unknowns.getElementAtIndex(8); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 844 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 957 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 904 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 328 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 965 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java | 911 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java | 741 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 851 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 726 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 1784 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 333 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java | 1817 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1047 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1043 |
inliersData = null;
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 818 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 763 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 826 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java | 770 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 794 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 740 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 801 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java | 746 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, initialMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1695 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1864 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1767 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 529 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1884 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1786 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java | 1411 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1706 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 1381 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3588 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 533 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3653 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2136 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2133 |
inliersData = null;
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 5176 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 2931 |
protected double computeError(final StandardDeviationFrameBodyKinematics measurement, final Matrix preliminaryMa) {
// We know that measured specific force is:
// fmeas = ba + (I + Ma) * ftrue
// Hence:
// [fmeasx] = [bx] + ( [1 0 0] + [sx mxy mxz]) [ftruex]
// [fmeasy] [by] [0 1 0] [myx sy myz] [ftruey]
// [fmeasz] [bz] [0 0 1] [mzx mzy sz ] [ftruez]
final var measuredKinematics = measurement.getKinematics();
final var ecefFrame = measurement.getFrame();
final var previousEcefFrame = measurement.getPreviousFrame();
final var timeInterval = measurement.getTimeInterval();
final var expectedKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(timeInterval, ecefFrame,
previousEcefFrame);
final var fMeasX1 = measuredKinematics.getFx();
final var fMeasY1 = measuredKinematics.getFy();
final var fMeasZ1 = measuredKinematics.getFz();
final var fTrueX = expectedKinematics.getFx();
final var fTrueY = expectedKinematics.getFy();
final var fTrueZ = expectedKinematics.getFz(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 5782 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 6324 |
tmp3.setElementAtIndex(2, fmeasZ - biasZ);
tmp2.multiply(tmp3, tmp4);
final var norm = Utils.normF(tmp4);
final var diff = groundTruthGravityNorm - norm;
return diff * diff;
} catch (final AlgebraException e) {
return Double.MAX_VALUE;
}
}
/**
* Computes a preliminary solution for a subset of samples picked by a robust estimator.
*
* @param samplesIndices indices of samples picked by the robust estimator.
* @param solutions list where estimated preliminary solution will be stored.
*/
protected void computePreliminarySolutions(final int[] samplesIndices, final List<PreliminaryResult> solutions) {
final var meas = new ArrayList<StandardDeviationBodyKinematics>();
for (final var samplesIndex : samplesIndices) {
meas.add(measurements.get(samplesIndex));
}
try {
final var result = new PreliminaryResult();
result.estimatedMa = getInitialMa(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/bias/BodyMagneticFluxDensityBiasEstimator.java | 1724 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedBodyKinematicsNoiseEstimator.java | 225 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedMeasurementNoiseEstimator.java | 182 |
| com/irurueta/navigation/inertial/calibration/noise/WindowedTriadNoiseEstimator.java | 202 |
this(position, nedC, convertTime(date), magneticModel, listener);
}
/**
* Gets time interval between body kinematics (IMU acceleration + gyroscope)
* samples expressed in seconds (s).
*
* @return time interval between body kinematics samples.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between body kinematics (IMU acceleration + gyroscope)
* samples expressed in seconds (s).
*
* @param timeInterval time interval between body kinematics samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval < 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between body kinematics (IMU acceleration + gyroscope)
* samples.
*
* @return time interval between body kinematics samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between body kinematics (IMU acceleration + gyroscope)
* samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between body kinematics (IMU acceleration + gyroscope)
* samples.
*
* @param timeInterval time interval between body kinematics samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(convertTime(timeInterval)); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4967 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4203 |
final var m22 = params[5];
final var m13 = params[6];
final var m23 = params[7];
final var m33 = params[8];
final var g11 = params[9];
final var g21 = params[10];
final var g31 = params[11];
final var g12 = params[12];
final var g22 = params[13];
final var g32 = params[14];
final var g13 = params[15];
final var g23 = params[16];
final var g33 = params[17];
return evaluate(i, bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4471 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7699 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5507 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6571 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5004 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8171 |
final ECEFPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6021 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7065 |
final NEDPosition position, final double turntableRotationRate, final double timeInterval,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias, final Matrix initialMg,
final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 7425 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 6422 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 7945 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 6909 |
if (identity == null) {
identity = Matrix.identity(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
}
if (tmp1 == null) {
tmp1 = new Matrix(BodyMagneticFluxDensity.COMPONENTS, BodyMagneticFluxDensity.COMPONENTS);
}
if (tmp2 == null) {
tmp2 = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
}
if (tmp3 == null) {
tmp3 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
if (tmp4 == null) {
tmp4 = new Matrix(BodyKinematics.COMPONENTS, 1);
}
identity.add(estMm, tmp1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/estimators/ECEFKinematicsEstimator.java | 1592 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 6720 |
final var alphaSkew1 = Utils.skewMatrix(new double[]{alphaX, alphaY, alphaZ});
if (alphaNorm > ALPHA_THRESHOLD) {
final var alphaNorm2 = alphaNorm * alphaNorm;
final var value1 = (1.0 - Math.cos(alphaNorm)) / alphaNorm2;
final var value2 = (1.0 - Math.sin(alphaNorm) / alphaNorm) / alphaNorm2;
final var tmp1 = alphaSkew1.multiplyByScalarAndReturnNew(value1);
final var tmp2 = alphaSkew1.multiplyByScalarAndReturnNew(value2);
tmp2.multiply(alphaSkew1);
final var tmp3 = Matrix.identity(ROWS, ROWS);
tmp3.add(tmp1);
tmp3.add(tmp2);
oldCbe.multiply(tmp3);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 6891 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 6938 |
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 6985 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 7033 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(), fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 7403 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 7451 |
final double timeInterval, final Point3D oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getInhomX(), oldPosition.getInhomY(), oldPosition.getInhomZ(),
oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final Point3D oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 7797 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 7845 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final ECEFVelocity oldVelocity,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC,
oldVelocity.getVx(), oldVelocity.getVy(), oldVelocity.getVz(), fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 7882 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 7918 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final BodyKinematics kinematics, final ECEFFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
kinematics.getAngularRateX(), kinematics.getAngularRateY(), kinematics.getAngularRateZ(), result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param kinematics body kinematics containing specific forces and angular rates applied to
* the body.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8128 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8175 |
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8449 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8493 |
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(), oldC,
oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8804 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8848 |
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(), oldC,
oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 3410 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 3458 |
final double timeInterval, final Point3D oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldPosition.getInhomX(), oldPosition.getInhomY(), oldPosition.getInhomZ(),
oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECI(
final Time timeInterval, final Point3D oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 3659 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 3695 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final BodyKinematics kinematics, final ECIFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz,
kinematics.getFx(), kinematics.getFy(), kinematics.getFz(),
kinematics.getAngularRateX(), kinematics.getAngularRateY(), kinematics.getAngularRateZ(), result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param kinematics body kinematics containing specific forces and angular rates applied to
* the body.
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECI(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17695 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 18719 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17697 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19692 |
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17698 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20171 |
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17750 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 18774 |
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19197 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20649 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19252 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20704 |
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19693 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21496 |
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20170 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21495 |
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20753 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20851 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20802 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20900 |
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21299 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21493 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21348 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21542 |
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 4302 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2370 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 1266 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1683 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1637 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 2363 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3446 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 1285 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 2367 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3408 |
setInitialCrossCouplingErrors(initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @return array containing coordinates of known bias.
*/
@Override
public double[] getBias() {
final var result = new double[BodyKinematics.COMPONENTS];
getBias(result);
return result;
}
/**
* Gets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param result instance where result data will be copied to.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void getBias(final double[] result) {
if (result.length != BodyKinematics.COMPONENTS) {
throw new IllegalArgumentException();
}
result[0] = biasX;
result[1] = biasY;
result[2] = biasZ;
}
/**
* Sets known bias as an array.
* Array values are expressed in meters per squared second (m/s^2).
*
* @param bias known bias to be set
* @throws LockedException if calibrator is currently running.
* @throws IllegalArgumentException if provided array does not have length 3.
*/
@Override
public void setBias(final double[] bias) throws LockedException {
if (running) {
throw new LockedException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6266 |
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4909 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6438 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3403 |
private double evaluateGeneral(final double[] params) throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m21 = params[4];
final var m31 = params[5];
final var m12 = params[6];
final var m22 = params[7];
final var m32 = params[8];
final var m13 = params[9];
final var m23 = params[10];
final var m33 = params[11]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseGravityNormAccelerometerCalibrator.java | 6363 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5895 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6575 |
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, bx); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 683 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1621 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1653 |
super(measurements, bias, commonAxisUsed, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Matrix>() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 4193 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 4432 |
final double[] qualityScores, final double[] bias, final boolean commonAxisUsed,
final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(bias, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(bias, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(bias, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, bias,
commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, bias, commonAxisUsed,
listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param bias known accelerometer bias.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided quality scores length is smaller than
* 4 samples.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double[] bias, final boolean commonAxisUsed, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2719 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3273 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2821 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2955 |
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2920 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3468 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final Matrix bias, final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2954 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3501 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3023 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3568 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final Matrix bias, final Matrix initialMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3372 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3502 |
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2662 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3171 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2753 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2873 |
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and
* is expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2840 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3350 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final Matrix bias, final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2872 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3382 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2937 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3447 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final Matrix bias, final Matrix initialMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3262 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3383 |
final boolean commonAxisUsed, final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3246 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3799 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3351 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3485 |
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3450 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3997 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3484 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4030 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3552 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4096 |
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3901 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4031 |
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Acceleration groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3185 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3689 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3279 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3400 |
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3367 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3871 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final RobustKnownPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3399 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3903 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final Matrix initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3463 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3967 |
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements,
final Matrix initialBias, final Matrix initialMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, initialMa);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3783 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3904 |
final boolean commonAxisUsed, final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/bias/BodyMagneticFluxDensityBiasEstimator.java | 1725 |
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedMeasurementNoiseEstimator.java | 109 |
| com/irurueta/navigation/inertial/calibration/noise/AccumulatedTriadNoiseEstimator.java | 144 |
}
/**
* Gets time interval between body kinematics (IMU acceleration + gyroscope)
* samples expressed in seconds (s).
*
* @return time interval between body kinematics samples.
*/
public double getTimeInterval() {
return timeInterval;
}
/**
* Sets time interval between body kinematics (IMU acceleration + gyroscope)
* samples expressed in seconds (s).
*
* @param timeInterval time interval between body kinematics samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final double timeInterval) throws LockedException {
if (running) {
throw new LockedException();
}
if (timeInterval < 0.0) {
throw new IllegalArgumentException();
}
this.timeInterval = timeInterval;
}
/**
* Gets time interval between body kinematics (IMU acceleration + gyroscope)
* samples.
*
* @return time interval between body kinematics samples.
*/
public Time getTimeIntervalAsTime() {
return new Time(timeInterval, TimeUnit.SECOND);
}
/**
* Gets time interval between body kinematics (IMU acceleration + gyroscope)
* samples.
*
* @param result instance where time interval will be stored.
*/
public void getTimeIntervalAsTime(final Time result) {
result.setValue(timeInterval);
result.setUnit(TimeUnit.SECOND);
}
/**
* Sets time interval between body kinematics (IMU acceleration + gyroscope)
* samples.
*
* @param timeInterval time interval between body kinematics samples.
* @throws LockedException if estimator is currently running.
*/
public void setTimeInterval(final Time timeInterval) throws LockedException {
setTimeInterval(convertTime(timeInterval)); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 5001 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6349 |
private double evaluateGeneral(final int i, final double[] params) throws EvaluationException {
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var m11 = params[3];
final var m21 = params[4];
final var m31 = params[5];
final var m12 = params[6];
final var m22 = params[7];
final var m32 = params[8];
final var m13 = params[9];
final var m23 = params[10];
final var m33 = params[11]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3360 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4683 |
b = invInitialM.multiplyAndReturnNew(bg);
fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
@Override
public int getNumberOfDimensions() {
// Before and after normalized gravity versors
return 2 * BodyKinematics.COMPONENTS;
}
@Override
public double[] createInitialParametersArray() {
final var initial = new double[GENERAL_UNKNOWNS_AND_CROSS_BIASES];
// cross coupling errors M
final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
for (var i = 0; i < num; i++) {
initial[i] = initialM.getElementAtIndex(i);
}
// g-dependent cross biases G
for (int i = 0, j = num; i < num; i++, j++) {
initial[j] = initialG.getElementAtIndex(i); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5799 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6675 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3500 |
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, biasX); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3991 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4376 |
System.arraycopy(buffer, 0, initial, 9, num);
return initial;
}
@Override
public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
final Matrix jacobian) {
// We know that:
// Ωmeasx = bx + Ωtruex + sx * Ωtruex + mxy * Ωtruey + mxz * Ωtruez + g11 * ftruex + g12 * ftruey + g13 * ftruez
// Ωmeasy = by + Ωtruey + sy * Ωtruey + myz * Ωtruez + g21 * ftruex * g22 * ftruey + g23 * ftruez
// Ωmeasz = bz + Ωtruez + sz * Ωtruez + g31 * ftruex + g32 * ftruey + g33 * ftruez
// Hence, the derivatives respect the parameters bx, by, bz,
// sx, sy, sz, mxy mxz, myz, g11, g12, g13, g21, g22, g23,
// g31, g32, and g33 is:
// d(Ωmeasx)/d(bx) = 1.0
// d(Ωmeasx)/d(by) = 0.0
// d(Ωmeasx)/d(bz) = 0.0
// d(Ωmeasx)/d(sx) = Ωtruex
// d(Ωmeasx)/d(sy) = 0.0
// d(Ωmeasx)/d(sz) = 0.0
// d(Ωmeasx)/d(mxy) = Ωtruey
// d(Ωmeasx)/d(mxz) = Ωtruez
// d(Ωmeasx)/d(myz) = 0.0
// d(Ωmeasx)/d(g11) = ftruex
// d(Ωmeasx)/d(g12) = ftruey
// d(Ωmeasx)/d(g13) = ftruez
// d(Ωmeasx)/d(g21) = 0.0
// d(Ωmeasx)/d(g22) = 0.0
// d(Ωmeasx)/d(g23) = 0.0
// d(Ωmeasx)/d(g31) = 0.0
// d(Ωmeasx)/d(g32) = 0.0
// d(Ωmeasx)/d(g33) = 0.0
// d(Ωmeasy)/d(bx) = 0.0
// d(Ωmeasy)/d(by) = 1.0
// d(Ωmeasy)/d(bz) = 0.0
// d(Ωmeasy)/d(sx) = 0.0
// d(Ωmeasy)/d(sy) = Ωtruey
// d(Ωmeasy)/d(sz) = 0.0
// d(Ωmeasy)/d(mxy) = 0.0
// d(Ωmeasy)/d(mxz) = 0.0
// d(Ωmeasy)/d(myz) = Ωtruez
// d(Ωmeasx)/d(g11) = 0.0
// d(Ωmeasx)/d(g12) = 0.0
// d(Ωmeasx)/d(g13) = 0.0
// d(Ωmeasx)/d(g21) = ftruex
// d(Ωmeasx)/d(g22) = ftruey
// d(Ωmeasx)/d(g23) = ftruez
// d(Ωmeasx)/d(g31) = 0.0
// d(Ωmeasx)/d(g32) = 0.0
// d(Ωmeasx)/d(g33) = 0.0
// d(Ωmeasz)/d(bx) = 0.0
// d(Ωmeasz)/d(by) = 0.0
// d(Ωmeasz)/d(bz) = 1.0
// d(Ωmeasz)/d(sx) = 0.0
// d(Ωmeasz)/d(sy) = 0.0
// d(Ωmeasz)/d(sz) = Ωtruez
// d(Ωmeasz)/d(mxy) = 0.0
// d(Ωmeasz)/d(mxz) = 0.0
// d(Ωmeasz)/d(myz) = 0.0
// d(Ωmeasx)/d(g11) = 0.0
// d(Ωmeasx)/d(g12) = 0.0
// d(Ωmeasx)/d(g13) = 0.0
// d(Ωmeasx)/d(g21) = 0.0
// d(Ωmeasx)/d(g22) = 0.0
// d(Ωmeasx)/d(g23) = 0.0
// d(Ωmeasx)/d(g31) = ftruex
// d(Ωmeasx)/d(g32) = ftruey
// d(Ωmeasx)/d(g33) = ftruez
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var sx = params[3];
final var sy = params[4];
final var sz = params[5];
final var mxy = params[6];
final var mxz = params[7];
final var myz = params[8]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3870 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4636 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 4245 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 4482 |
final double[] qualityScores, final double[] bias, final boolean commonAxisUsed,
final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(bias, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(bias, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(bias, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, bias, commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, bias, commonAxisUsed, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param bias known gyroscope bias.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided quality scores length is smaller
* than 6 samples.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double[] bias, final boolean commonAxisUsed, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3380 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4152 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final double[] initialBias, final Matrix initialMg, final Matrix initialGg,
final double[] accelerometerBias, final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(
sequences, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4748 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5784 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 7017 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8145 |
final Matrix initialGg, final double[] accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 4873 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 5910 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 6881 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 8009 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustKnownBiasTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustKnownBiasTurntableGyroscopeCalibrator(
position, turntableRotationRate, timeInterval, measurements, bias, initialMg, initialGg,
accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustKnownBiasTurntableGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5282 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6299 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7499 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8607 |
final Matrix initialMg, final Matrix initialGg, final double[] accelerometerBias,
final Matrix accelerometerMa, final RobustTurntableGyroscopeCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 5401 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 6418 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 7369 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 8477 |
final Matrix initialGg, final Matrix accelerometerBias, final Matrix accelerometerMa,
final RobustTurntableGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case LMEDS -> new LMedSRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case MSAC -> new MSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval,
measurements, initialBias, initialMg, initialGg, accelerometerBias, accelerometerMa, listener);
case PROSAC -> new PROSACRobustTurntableGyroscopeCalibrator(position, turntableRotationRate, timeInterval, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 537 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3621 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1722 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3585 |
public void setListener(final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the estimator.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or no.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Gets Earth's magnetic model.
*
* @return Earth's magnetic model or null if not provided.
*/
public WorldMagneticModel getMagneticModel() {
return magneticModel;
}
/**
* Sets Earth's magnetic model.
* If not provided a default model will be loaded internally.
*
* @param magneticModel Earth's magnetic model to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
if (running) {
throw new LockedException();
}
this.magneticModel = magneticModel;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if calibration fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2910 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 3043 |
final double[] hardIron,
final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 3590 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 3731 |
final double[] hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron array does not have length 3.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] hardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 4243 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 4393 |
final double[] hardIron,
final RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided quality
* scores length is smaller than 10 samples.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2982 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3567 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3088 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3229 |
final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3192 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3777 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3228 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3813 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3302 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3887 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix hardIron, final Matrix initialMm, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, initialMm);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3673 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3814 |
final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 3421 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 3554 |
final double[] initialHardIron,
final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4101 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4242 |
final double[] initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron array does not have length 3.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final double[] initialHardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4755 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4905 |
final double[] initialHardIron,
final RobustKnownMagneticFluxDensityNormMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided quality
* scores length is smaller than 10 samples.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3491 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4070 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3596 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3737 |
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] initialHardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3700 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4279 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix initialHardIron, final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3736 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4315 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final Matrix initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3810 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4389 |
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final Matrix initialHardIron, final Matrix initialMm, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, initialMm);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4175 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4316 |
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] initialHardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2073 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2117 |
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3111 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4040 |
estimatedMa.setElementAt(2, 2, sz);
estimatedCovariance = fitter.getCovar();
// propagate covariance matrix so that all parameters are taken into
// account in the order: sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy
// We define a lineal function mapping original parameters for the common
// axis case to the general case
// [sx'] = [1 0 0 0 0 0][sx]
// [sy'] [0 1 0 0 0 0][sy]
// [sz'] [0 0 1 0 0 0][sz]
// [mxy'] [0 0 0 1 0 0][mxy]
// [mxz'] [0 0 0 0 1 0][mxz]
// [myx'] [0 0 0 0 0 0][myz]
// [myz'] [0 0 0 0 0 1]
// [mzx'] [0 0 0 0 0 0]
// [mzy'] [0 0 0 0 0 0]
// As defined in com.irurueta.statistics.MultivariateNormalDist,
// if we consider the jacobian of the lineal application the matrix shown
// above, then covariance can be propagated as follows
final var jacobian = Matrix.identity(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
jacobian.setElementAt(5, 5, 0.0);
jacobian.setElementAt(6, 5, 1.0);
// propagated covariance is J * Cov * J'
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Internal method to perform general calibration.
*
* @throws AlgebraException if there are numerical errors.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateGeneral() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java | 2729 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java | 3010 |
final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ,
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) {
this(measurements, commonAxisUsed, biasX, biasY, biasZ, initialSx, initialSy, initialSz,
initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
try {
setPosition(position);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param biasX x-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param biasY y-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param biasZ z-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownBiasAndPositionAccelerometerCalibrator(
final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double biasX, final double biasY, final double biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3857 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4604 |
estimatedMa.setElementAt(2, 2, sz);
estimatedCovariance = fitter.getCovar();
// propagate covariance matrix so that all parameters are taken into
// account in the order: bx, by, bz, sx, sy, sz, mxy, mxz, myx,
// myz, mzx, mzy.
// We define a lineal function mapping original parameters for the common
// axis case to the general case
// [bx'] = [1 0 0 0 0 0 0 0 0][bx]
// [by'] [0 1 0 0 0 0 0 0 0][by]
// [bz'] [0 0 1 0 0 0 0 0 0][bz]
// [sx'] [0 0 0 1 0 0 0 0 0][sx]
// [sy'] [0 0 0 0 1 0 0 0 0][sy]
// [sz'] [0 0 0 0 0 1 0 0 0][sz]
// [mxy'] [0 0 0 0 0 0 1 0 0][mxy]
// [mxz'] [0 0 0 0 0 0 0 1 0][mxz]
// [myx'] [0 0 0 0 0 0 0 0 0][myz]
// [myz'] [0 0 0 0 0 0 0 0 1]
// [mzx'] [0 0 0 0 0 0 0 0 0]
// [mzy'] [0 0 0 0 0 0 0 0 0]
// As defined in com.irurueta.statistics.MultivariateNormalDist,
// if we consider the jacobian of the lineal application the matrix shown
// above, then covariance can be propagated as follows
final var jacobian = Matrix.identity(GENERAL_UNKNOWNS, COMMON_Z_AXIS_UNKNOWNS);
jacobian.setElementAt(8, 8, 0.0);
jacobian.setElementAt(9, 8, 1.0);
// propagated covariance is J * Cov * J'
final var jacobianTrans = jacobian.transposeAndReturnNew();
jacobian.multiply(estimatedCovariance);
jacobian.multiply(jacobianTrans);
estimatedCovariance = jacobian;
estimatedChiSq = fitter.getChisq();
estimatedMse = fitter.getMse();
}
/**
* Internal method to perform general calibration.
*
* @throws AlgebraException if there are numerical errors.
* @throws FittingException if no convergence to solution is found.
* @throws com.irurueta.numerical.NotReadyException if fitter is not ready.
*/
private void calibrateGeneral() throws AlgebraException, FittingException,
com.irurueta.numerical.NotReadyException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java | 3016 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java | 3325 |
final double initialBiasZ, final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) {
this(measurements, commonAxisUsed, initialBiasX, initialBiasY, initialBiasZ, initialSx, initialSy, initialSz,
initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
try {
setPosition(position);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBiasX initial x-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialBiasY initial y-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialBiasZ initial z-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
* @param listener listener to handle events raised by this calibrator.
*/
public KnownPositionAccelerometerCalibrator(
final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
final boolean commonAxisUsed, final double initialBiasX, final double initialBiasY, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2610 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2843 |
final double biasX, final double biasY, final double biasZ,
final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
listener);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
listener);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param biasX known x coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasY known y coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasZ known z coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3638 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 3913 |
final double[] qualityScores, final double biasX, final double biasY, final double biasZ,
final boolean commonAxisUsed, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ,
commonAxisUsed);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, commonAxisUsed);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
commonAxisUsed);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, biasX, biasY, biasZ,
commonAxisUsed);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param biasX known x coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasY known y coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasZ known z coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 4 samples.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final double[] qualityScores, final double biasX, final double biasY, final double biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2856 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3535 |
final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 2989 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3406 |
final boolean commonAxisUsed, final Matrix bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3901 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4706 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4059 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4551 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2786 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3415 |
final boolean commonAxisUsed, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 2905 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3295 |
final boolean commonAxisUsed, final Matrix bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3761 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4512 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3910 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4363 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3387 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4064 |
final boolean commonAxisUsed, final double[] initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3519 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 3936 |
final boolean commonAxisUsed, final Matrix initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3 or
* if provided gravity norm value is negative.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4427 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5232 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4587 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5077 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3313 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3936 |
final boolean commonAxisUsed, final double[] initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(position,
measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3432 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 3817 |
final boolean commonAxisUsed, final Matrix initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param initialMa initial scale factors and cross coupling errors matrix.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if either provided bias matrix is not 3x1 or
* scaling and coupling error matrix is not 3x3.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final ECEFPosition position, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4273 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5015 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4426 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4865 |
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4967 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5607 |
final var m22 = params[5];
final var m13 = params[6];
final var m23 = params[7];
final var m33 = params[8];
final var g11 = params[9];
final var g21 = params[10];
final var g31 = params[11];
final var g12 = params[12];
final var g22 = params[13];
final var g32 = params[14];
final var g13 = params[15];
final var g23 = params[16];
final var g33 = params[17];
return evaluate(i, bx, by, bz, m11, 0.0, 0.0, m12, m22, 0.0, m13, m23, m33, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 5128 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5801 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5897 |
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
b.setElementAtIndex(0, bx); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 4203 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 6404 |
final var m32 = params[5];
final var m13 = params[6];
final var m23 = params[7];
final var m33 = params[8];
final var g11 = params[9];
final var g21 = params[10];
final var g31 = params[11];
final var g12 = params[12];
final var g22 = params[13];
final var g32 = params[14];
final var g13 = params[15];
final var g23 = params[16];
final var g33 = params[17];
return evaluate(i, m11, m21, m31, m12, m22, m32, m13, m23, m33, g11, g21, g31, g12, g22, g32, g13, g23, g33); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustEasyGyroscopeCalibrator.java | 742 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 727 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 334 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 333 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1052 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1051 |
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustEasyGyroscopeCalibrator.java | 1412 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasEasyGyroscopeCalibrator.java | 1382 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 537 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 535 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2146 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2142 |
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException | AlgebraException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2678 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2907 |
final double biasX, final double biasY, final double biasZ,
final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
listener);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
listener);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, biasX, biasY, biasZ,
listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param biasX known x coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3697 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 3969 |
final double biasZ, final boolean commonAxisUsed, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, biasX, biasY, biasZ, commonAxisUsed);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, biasX, biasY, biasZ, commonAxisUsed);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param biasX known x coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 6 samples.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final double[] qualityScores, final double biasX, final double biasY, final double biasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2734 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseMagneticFluxDensityNormMagnetometerCalibrator.java | 3518 |
invM.multiply(bm, b);
invM.multiply(bmeas, btrue);
btrue.subtract(b);
final var norm = Utils.normF(btrue);
return norm * norm;
} catch (final AlgebraException e) {
throw new EvaluationException(e);
}
}
/**
* Converts magnetic flux density value and unit to Teslas.
*
* @param value magnetic flux density value.
* @param unit unit of magnetic flux density value.
* @return converted value.
*/
private static double convertMagneticFluxDensity(final double value, final MagneticFluxDensityUnit unit) {
return MagneticFluxDensityConverter.convert(value, unit, MagneticFluxDensityUnit.TESLA);
}
/**
* Converts magnetic flux density instance to Teslas.
*
* @param magneticFluxDensity magnetic flux density instance to be converted.
* @return converted value.
*/
private static double convertMagneticFluxDensity(final MagneticFluxDensity magneticFluxDensity) {
return convertMagneticFluxDensity(magneticFluxDensity.getValue().doubleValue(), magneticFluxDensity.getUnit());
}
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3124 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3850 |
final boolean commonAxisUsed, final double[] hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3265 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 3709 |
final boolean commonAxisUsed, final Matrix hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4430 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5263 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4589 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5101 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3632 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4352 |
final boolean commonAxisUsed, final double[] initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 3773 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4211 |
final boolean commonAxisUsed, final Matrix initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param initialMm initial soft-iron matrix containing scale factors
* and cross coupling errors.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if soft-iron matrix is not
* 3x3.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final NEDPosition position, final List<StandardDeviationBodyMagneticFluxDensity> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4918 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5747 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5077 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5584 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2072 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8803 |
public void navigate(
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2116 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8847 |
public void navigate(
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 13491 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 15343 |
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(),
oldPosition.getHeight(), oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 13492 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 15725 |
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(),
oldPosition.getHeight(), oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 13591 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 14398 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
* along north, east and down axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 13593 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 15439 |
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
* along north, east and down axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 13594 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 15820 |
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
oldVelocity.getVn(), oldVelocity.getVe(), oldVelocity.getVd(), fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
* along north, east and down axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 15344 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 16665 |
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(),
oldC, oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold,
result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 15724 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 16664 |
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(timeInterval, oldPosition.getLatitude(), oldPosition.getLongitude(), oldPosition.getHeight(), oldC,
oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 5332 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2717 |
fmeas.setElementAtIndex(2, fmeasZ);
m.setElementAt(0, 0, m11);
m.setElementAt(1, 0, m21);
m.setElementAt(2, 0, m31);
m.setElementAt(0, 1, m12);
m.setElementAt(1, 1, m22);
m.setElementAt(2, 1, m32);
m.setElementAt(0, 2, m13);
m.setElementAt(1, 2, m23);
m.setElementAt(2, 2, m33);
Utils.inverse(m, invM);
// b = M^-1*ba
invM.multiply(ba, b); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2718 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 2940 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasAndFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3055 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3188 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3680 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3644 |
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
running = true;
if (listener != null) {
listener.onCalibrateStart(this);
}
if (commonAxisUsed) {
calibrateCommonAxis();
} else {
calibrateGeneral();
}
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final AlgebraException | FittingException | com.irurueta.numerical.NotReadyException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 845 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 958 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 905 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownFrameAccelerometerCalibrator.java | 329 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 966 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java | 912 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 852 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 1785 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustKnownFrameGyroscopeCalibrator.java | 334 |
| com/irurueta/navigation/inertial/calibration/gyroscope/LMedSRobustTurntableGyroscopeCalibrator.java | 1818 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownFrameMagnetometerCalibrator.java | 334 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 333 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1049 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1052 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1045 |
| com/irurueta/navigation/inertial/calibration/magnetometer/LMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1051 |
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
innerEstimator.setStopThreshold(stopThreshold);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1696 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1865 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1768 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownFrameAccelerometerCalibrator.java | 530 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownGravityNormAccelerometerCalibrator.java | 1885 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROMedSRobustKnownPositionAccelerometerCalibrator.java | 1787 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1707 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownBiasTurntableGyroscopeCalibrator.java | 3589 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustKnownFrameGyroscopeCalibrator.java | 534 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROMedSRobustTurntableGyroscopeCalibrator.java | 3654 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownFrameMagnetometerCalibrator.java | 537 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 535 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2138 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2146 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2135 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2142 |
innerEstimator.setUseInlierThresholds(true);
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3861 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4512 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3726 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4325 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias);
};
}
/**
* Creates a robust accelerometer calibrator
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if quality scores array is smaller than 10
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4386 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5037 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4237 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4829 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3613 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4929 |
initial[i] = initialB.getElementAtIndex(i);
}
// upper diagonal cross coupling errors M
var k = BodyKinematics.COMPONENTS;
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
// g-dependent cross biases G
final var num = BodyKinematics.COMPONENTS * BodyKinematics.COMPONENTS;
for (int i = 0, j = k; i < num; i++, j++) {
initial[j] = initialG.getElementAtIndex(i); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 3665 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3415 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4742 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4984 |
final var m22 = result[5];
final var m13 = result[6];
final var m23 = result[7];
final var m33 = result[8];
final var g11 = result[9];
final var g21 = result[10];
final var g31 = result[11];
final var g12 = result[12];
final var g22 = result[13];
final var g32 = result[14];
final var g13 = result[15];
final var g23 = result[16];
final var g33 = result[17];
final var b = new Matrix(BodyKinematics.COMPONENTS, 1); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4129 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4931 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3639 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4451 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final double[] initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4389 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5060 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4877 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5543 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 3154 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 13348 |
final var skewAlpha = Utils.skewMatrix(alphaIbb);
// Obtain coordinate transformation matrix from the new attitude to the old
// using Rodrigues' formula, (5.73)
final var cNewOld = Matrix.identity(ROWS, ROWS);
if (magAlpha > ALPHA_THRESHOLD) {
final var magAlpha2 = magAlpha * magAlpha;
final var value1 = Math.sin(magAlpha) / magAlpha;
final var value2 = (1.0 - Math.cos(magAlpha)) / magAlpha2;
final var tmp1 = skewAlpha.multiplyByScalarAndReturnNew(value1);
final var tmp2 = skewAlpha.multiplyByScalarAndReturnNew(value2);
tmp2.multiply(skewAlpha);
cNewOld.add(tmp1);
cNewOld.add(tmp2);
} else {
cNewOld.add(skewAlpha);
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 160 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 264 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 1446 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 1550 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 2104 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 2208 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 2780 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 2878 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 3520 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 3618 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4266 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4370 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final NEDFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4462 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4554 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4646 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 4738 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 5339 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 5431 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 5703 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 5795 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final NEDFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigate(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, DEFAULT_ACCURACY_THRESHOLD, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 13437 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 14349 |
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 14813 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 16060 |
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 16159 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 16256 |
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC,
oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 16616 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 16804 |
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
navigateNED(convertTimeToDouble(timeInterval), oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateNED(
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/INSGNSSTightlyCoupledKalmanFilteredEstimator.java | 1348 |
| com/irurueta/navigation/inertial/INSLooselyCoupledKalmanFilteredEstimator.java | 1225 |
}
/**
* Gets last provided user kinematics containing applied specific force and
* angular rates resolved in body axes.
*
* @return last provided user kinematics.
*/
public BodyKinematics getKinematics() {
return kinematics != null ? new BodyKinematics(kinematics) : null;
}
/**
* Gets last provided user kinematics containing applied specific force and
* angular rates resolved in body axes.
*
* @param result instance where last provided body kinematics will be stored.
* @return true if provided result instance was updated, false otherwise.
*/
public boolean getKinematics(final BodyKinematics result) {
if (kinematics != null) {
result.copyFrom(kinematics);
return true;
} else {
return false;
}
}
/**
* Gets corrected kinematics which are the last provided user kinematics after
* removal of the biases estimated by the Kalman filter.
*
* @return corrected kinematics.
* @see #getKinematics()
*/
public BodyKinematics getCorrectedKinematics() {
return correctedKinematics != null ? new BodyKinematics(correctedKinematics) : null;
}
/**
* Gets corrected kinematics which are the last provided user kinematics after
* removal of the biases estimated by the Kalman filter.
*
* @param result instance where corrected body kinematics will be stored.
* @return true if provided result instance was updated, false otherwise.
*/
public boolean getCorrectedKinematics(final BodyKinematics result) {
if (correctedKinematics != null) {
result.copyFrom(correctedKinematics);
return true;
} else {
return false;
}
}
/**
* Gets current estimation containing user ECEF position, user ECEF velocity,
* clock offset and clock drift.
*
* @return current estimation containing user ECEF position, user ECEF velocity,
* clock offset and clock drift.
*/
public GNSSEstimation getEstimation() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3722 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3978 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 3993 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownFrameGyroscopeNonLinearLeastSquaresCalibrator.java | 4378 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4468 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4726 |
return initial;
}
@Override
public void evaluate(final int i, final double[] point, final double[] result, final double[] params,
final Matrix jacobian) {
// We know that:
// fmeasx = bx + ftruex + sx * ftruex + mxy * ftruey + mxz * ftruez
// fmeasy = by + ftruey + sy * ftruey + myz * ftruez
// fmeasz = bz + ftruez + sz * ftruez
// Hence, the derivatives respect the parameters bx, by, bz, sx, sy,
// sz, mxy, mxz, myz
// d(fmeasx)/d(bx) = 1.0
// d(fmeasx)/d(by) = 0.0
// d(fmeasx)/d(bz) = 0.0
// d(fmeasx)/d(sx) = ftruex
// d(fmeasx)/d(sy) = 0.0
// d(fmeasx)/d(sz) = 0.0
// d(fmeasx)/d(mxy) = ftruey
// d(fmeasx)/d(mxz) = ftruez
// d(fmeasx)/d(myz) = 0.0
// d(fmeasy)/d(bx) = 0.0
// d(fmeasy)/d(by) = 1.0
// d(fmeasy)/d(bz) = 0.0
// d(fmeasy)/d(sx) = 0.0
// d(fmeasy)/d(sy) = ftruey
// d(fmeasy)/d(sz) = 0.0
// d(fmeasy)/d(mxy) = 0.0
// d(fmeasy)/d(mxz) = 0.0
// d(fmeasy)/d(myz) = ftruez
// d(fmeasz)/d(bx) = 0.0
// d(fmeasz)/d(by) = 0.0
// d(fmeasz)/d(bz) = 1.0
// d(fmeasz)/d(sx) = 0.0
// d(fmeasz)/d(sy) = 0.0
// d(fmeasz)/d(sz) = ftruez
// d(fmeasz)/d(mxy) = 0.0
// d(fmeasz)/d(mxz) = 0.0
// d(fmeasz)/d(myz) = 0.0
final var bx = params[0];
final var by = params[1];
final var bz = params[2];
final var sx = params[3];
final var sy = params[4];
final var sz = params[5];
final var mxy = params[6];
final var mxz = params[7];
final var myz = params[8]; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4704 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5287 |
for (int i = 0, j = num; i < num; i++, j++) {
initial[j] = initG.getElementAtIndex(i);
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException {
measAngularRateX = point[0];
measAngularRateY = point[1];
measAngularRateZ = point[2];
fmeasX = point[3];
fmeasY = point[4];
fmeasZ = point[5];
gradientEstimator.gradient(params, derivatives);
return evaluateGeneralWitGDependentCrossBiases(params); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerLinearLeastSquaresCalibrator.java | 537 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3621 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerLinearLeastSquaresCalibrator.java | 1722 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 3585 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 1364 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 1328 |
public void setListener(final KnownFrameMagnetometerLinearLeastSquaresCalibratorListener listener)
throws LockedException {
if (running) {
throw new LockedException();
}
this.listener = listener;
}
/**
* Gets minimum number of required measurements.
*
* @return minimum number of required measurements.
*/
@Override
public int getMinimumRequiredMeasurements() {
return MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is ready to start the estimator.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return measurements != null && measurements.size() >= MINIMUM_MEASUREMENTS;
}
/**
* Indicates whether calibrator is currently running or no.
*
* @return true if calibrator is running, false otherwise.
*/
@Override
public boolean isRunning() {
return running;
}
/**
* Gets Earth's magnetic model.
*
* @return Earth's magnetic model or null if not provided.
*/
public WorldMagneticModel getMagneticModel() {
return magneticModel;
}
/**
* Sets Earth's magnetic model.
* If not provided a default model will be loaded internally.
*
* @param magneticModel Earth's magnetic model to be set.
* @throws LockedException if calibrator is currently running.
*/
public void setMagneticModel(final WorldMagneticModel magneticModel) throws LockedException {
if (running) {
throw new LockedException();
}
this.magneticModel = magneticModel;
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 7118 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8663 |
final ECEFVelocity oldVelocity, final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(), oldC, oldVelocity,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param kinematics body kinematics containing specific forces and angular rates applied to
* the body.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static void navigateECEF(
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final BodyKinematics kinematics, final ECEFFrame result) throws InertialNavigatorException, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 6657 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 6761 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public NEDFrame navigateAndReturnNew(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 7947 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 8051 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
oldVn, oldVe, oldVd, fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public NEDFrame navigateAndReturnNew(
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 8605 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 8711 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
accuracyThreshold);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public NEDFrame navigateAndReturnNew(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 9288 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 9386 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public NEDFrame navigateAndReturnNew(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 10028 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 10126 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public NEDFrame navigateAndReturnNew(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 10771 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 10877 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
accuracyThreshold);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public NEDFrame navigateAndReturnNew(
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 10977 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 11071 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
accuracyThreshold);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public NEDFrame navigateAndReturnNew(
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 11165 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 11259 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC,
oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz, angularRateX, angularRateY, angularRateZ,
accuracyThreshold);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public NEDFrame navigateAndReturnNew(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 11861 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 11953 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public NEDFrame navigateAndReturnNew(
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 12225 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 12317 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
return navigateNEDAndReturnNew(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold);
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public NEDFrame navigateAndReturnNew(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17695 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 18774 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17750 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 18719 |
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17804 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17858 |
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17909 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17960 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
* along north, east and down axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 18825 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 18876 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
* along north, east and down axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19197 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20704 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19252 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20649 |
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19306 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19360 |
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19793 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19844 |
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19892 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 19940 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
* along north, east and down axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20271 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20322 |
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
final double oldVn, final double oldVe, final double oldVd,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20370 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20418 |
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight,
final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
* along north, east and down axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20753 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20900 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20802 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20851 |
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldSpeedN, oldSpeedE, oldSpeedD,
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldLatitude previous latitude expressed in radians (rad).
* @param oldLongitude previous longitude expressed in radians (rad).
* @param oldHeight previous height expressed in meters (m).
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final double timeInterval, final double oldLatitude, final double oldLongitude, final double oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20948 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 20996 |
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
final Speed oldSpeedN, final Speed oldSpeedE, final Speed oldSpeedD,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldPosition, oldC, oldSpeedN, oldSpeedE, oldSpeedD, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldSpeedN previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedE previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param oldSpeedD previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21041 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21086 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final NEDVelocity oldVelocity,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVelocity, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVelocity previous velocity of body frame with respect ECEF frame resolved
* along north, east and down axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21299 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21542 |
final double timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs.
* @param oldLatitude previous latitude angle.
* @param oldLongitude previous longitude angle.
* @param oldHeight previous height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21348 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21493 |
final Time timeInterval, final Angle oldLatitude, final Angle oldLongitude, final Distance oldHeight,
final CoordinateTransformation oldC, final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldLatitude, oldLongitude, oldHeight, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21396 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 21444 |
final double timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC,
final double oldVn, final double oldVe, final double oldVd,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final double accuracyThreshold) throws InertialNavigatorException,
InvalidSourceAndDestinationFrameTypeException {
final var result = new NEDFrame();
navigateNED(timeInterval, oldPosition, oldC, oldVn, oldVe, oldVd, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
return result;
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous curvilinear position expressed in terms of latitude,
* longitude and height.
* @param oldC previous body-to-NED coordinate transformation.
* @param oldVn previous velocity north-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVe previous velocity east-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param oldVd previous velocity down-coordinate of body frame with respect ECEF frame,
* resolved along NED-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param accuracyThreshold threshold to determine whether a matrix is a valid rotation or not.
* @return estimated NED frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-NED-frame coordinate transformation matrix are
* invalid.
*/
public static NEDFrame navigateNEDAndReturnNew(
final Time timeInterval, final NEDPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3077 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronAndFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4006 |
jacobian.setElementAt(2, 2, ftruez);
jacobian.setElementAt(2, 3, 0.0);
jacobian.setElementAt(2, 4, 0.0);
jacobian.setElementAt(2, 5, 0.0);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var sx = result[0];
final var sy = result[1];
final var sz = result[2];
final var mxy = result[3];
final var mxz = result[4];
final var myz = result[5];
if (estimatedMa == null) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java | 2543 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java | 2580 |
this(biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz, initialMyx,
initialMyz, initialMzx, initialMzy);
try {
setPosition(position);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param biasX x-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param biasY y-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param biasZ z-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
*/
public KnownBiasAndPositionAccelerometerCalibrator(
final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java | 2615 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java | 2905 |
final double biasX, final double biasY, final double biasZ, final double initialSx, final double initialSy,
final double initialSz, final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy,
final KnownBiasAndPositionAccelerometerCalibrationListener listener) {
this(measurements, biasX, biasY, biasZ, initialSx, initialSy, initialSz, initialMxy, initialMxz,
initialMyx, initialMyz, initialMzx, initialMzy, listener);
try {
setPosition(position);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param position position where body kinematics measures have been taken.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param biasX x-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param biasY y-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param biasZ z-coordinate of accelerometer bias.
* This is expressed in meters per squared second (m/s^2).
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
*/
public KnownBiasAndPositionAccelerometerCalibrator(
final ECEFPosition position, final boolean commonAxisUsed, final double biasX, final double biasY, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownBiasAndPositionAccelerometerCalibrator.java | 5250 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java | 5748 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 1844 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 1891 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 3860 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasTurntableGyroscopeCalibrator.java | 3820 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustTurntableGyroscopeCalibrator.java | 3872 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 3907 |
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @return position where body kinematics measures have been taken or null if
* not available.
*/
public NEDPosition getNedPosition() {
final var result = new NEDPosition();
return getNedPosition(result) ? result : null;
}
/**
* Gets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param result instance where result will be stored.
* @return true if NED position could be computed, false otherwise.
*/
public boolean getNedPosition(final NEDPosition result) {
if (position != null) {
final var velocity = new NEDVelocity();
ECEFtoNEDPositionVelocityConverter.convertECEFtoNED(
position.getX(), position.getY(), position.getZ(),
0.0, 0.0, 0.0, result, velocity);
return true;
} else {
return false;
}
}
/**
* Sets position where body kinematics measures have been taken expressed in
* NED coordinates.
*
* @param position position where body kinematics measures have been taken.
* @throws LockedException if calibrator is currently running.
*/
public void setPosition(final NEDPosition position) throws LockedException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java | 2810 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java | 2851 |
this(initialBiasX, initialBiasY, initialBiasZ, initialSx, initialSy, initialSz,
initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy);
try {
setPosition(position);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBiasX initial x-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialBiasY initial y-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialBiasZ initial z-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
*/
public KnownPositionAccelerometerCalibrator(
final ECEFPosition position, final Collection<StandardDeviationBodyKinematics> measurements,
final double initialBiasX, final double initialBiasY, final double initialBiasZ,
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java | 2889 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownPositionAccelerometerCalibrator.java | 3207 |
final double initialBiasX, final double initialBiasY, final double initialBiasZ,
final double initialSx, final double initialSy, final double initialSz,
final double initialMxy, final double initialMxz, final double initialMyx,
final double initialMyz, final double initialMzx, final double initialMzy,
final KnownPositionAccelerometerCalibratorListener listener) {
this(measurements, initialBiasX, initialBiasY, initialBiasZ, initialSx, initialSy, initialSz,
initialMxy, initialMxz, initialMyx, initialMyz, initialMzx, initialMzy, listener);
try {
setPosition(position);
} catch (final LockedException ignore) {
// never happens
}
}
/**
* Constructor.
*
* @param position position where body kinematics measures have been taken.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBiasX initial x-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialBiasY initial y-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialBiasZ initial z-coordinate of accelerometer bias to be used
* to find a solution. This is expressed in meters per squared
* second (m/s^2).
* @param initialSx initial x scaling factor.
* @param initialSy initial y scaling factor.
* @param initialSz initial z scaling factor.
* @param initialMxy initial x-y cross coupling error.
* @param initialMxz initial x-z cross coupling error.
* @param initialMyx initial y-x cross coupling error.
* @param initialMyz initial y-z cross coupling error.
* @param initialMzx initial z-x cross coupling error.
* @param initialMzy initial z-y cross coupling error.
*/
public KnownPositionAccelerometerCalibrator(
final ECEFPosition position, final boolean commonAxisUsed,
final double initialBiasX, final double initialBiasY, final double initialBiasZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 731 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 1651 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 804 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 218 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 935 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 486 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 2091 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronAndFrameMagnetometerCalibrator.java | 289 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 1009 |
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final Matrix currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 843 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 791 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 213 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 850 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java | 797 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 1814 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 1718 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownFrameAccelerometerCalibrator.java | 480 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownGravityNormAccelerometerCalibrator.java | 1834 |
| com/irurueta/navigation/inertial/calibration/accelerometer/PROSACRobustKnownPositionAccelerometerCalibrator.java | 1740 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 914 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 861 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownFrameAccelerometerCalibrator.java | 285 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownGravityNormAccelerometerCalibrator.java | 923 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RANSACRobustKnownPositionAccelerometerCalibrator.java | 870 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 739 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1670 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 219 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1702 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 1655 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 3540 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownFrameGyroscopeCalibrator.java | 484 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustTurntableGyroscopeCalibrator.java | 3603 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 808 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1741 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownFrameGyroscopeCalibrator.java | 290 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustTurntableGyroscopeCalibrator.java | 1773 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownFrameMagnetometerCalibrator.java | 216 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 934 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 928 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 933 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownFrameMagnetometerCalibrator.java | 483 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2085 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 2083 |
| com/irurueta/navigation/inertial/calibration/magnetometer/PROSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 2087 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownFrameMagnetometerCalibrator.java | 287 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1002 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1000 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RANSACRobustKnownPositionAndInstantMagnetometerCalibrator.java | 1005 |
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return measurements.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(measurements.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2554 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 2792 |
final double biasX, final double biasY, final double biasZ,
final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(biasX, biasY, biasZ, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasY known y coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param biasZ known z coordinate of accelerometer bias expressed in meters per
* squared second (m/s^2).
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 4226 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 4465 |
final double[] bias, final boolean commonAxisUsed, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias,
commonAxisUsed);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias, commonAxisUsed);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements, bias,
commonAxisUsed);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements, bias,
commonAxisUsed);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param bias known accelerometer bias.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided quality scores length is smaller than
* 4 samples.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double[] bias, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 4260 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 4498 |
final double[] bias, final boolean commonAxisUsed,
final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias, commonAxisUsed,
listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias, commonAxisUsed,
listener);
case MSAC -> new MSACRobustKnownBiasAndFrameAccelerometerCalibrator(measurements, bias, commonAxisUsed,
listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements, bias,
commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameAccelerometerCalibrator(qualityScores, measurements, bias,
commonAxisUsed, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param bias known accelerometer bias.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided quality scores length is smaller than
* 4 samples.
*/
public static RobustKnownBiasAndFrameAccelerometerCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3745 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4399 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements,
final RobustKnownBiasAndGravityNormAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, listener);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, listener);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, listener);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, listener);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements list of body kinematics measurements taken at a given position with
* different unknown orientations and containing the standard deviations
* of accelerometer and gyroscope measurements.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3783 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4436 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements list of body kinematics measurements taken at a given position with
* different unknown orientations and containing the standard deviations
* of accelerometer and gyroscope measurements.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to be notified of events such as when estimation
* starts, ends or its progress significantly changes.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores,
final Double groundTruthGravityNorm, final List<StandardDeviationBodyKinematics> measurements, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3862 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4021 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4020 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4668 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4513 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4669 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is smaller
* than 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Acceleration groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3614 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4211 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements,
final RobustKnownBiasAndPositionAccelerometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, listener);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, listener);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, listener);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, listener);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements list of body kinematics measurements taken at a given position with
* different unknown orientations and containing the standard deviations
* of accelerometer and gyroscope measurements.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope. If true 7 samples are
* required, otherwise 10.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than the minimum number of
* required samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3650 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4249 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements list of body kinematics measurements taken at a given position with
* different unknown orientations and containing the standard deviations
* of accelerometer and gyroscope measurements.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope. If true 7 samples are
* required, otherwise 10.
* @param listener listener to be notified of events such as when estimation
* starts, ends or its progress significantly changes.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than the minimum number of
* required samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3727 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3877 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias);
};
}
/**
* Creates a robust accelerometer calibrator
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if quality scores array is smaller than 10
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3876 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4478 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(position, measurements, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
* quality scores array is smaller than 10
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4326 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4479 |
final List<StandardDeviationBodyKinematics> measurements, final double[] bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if quality scores array is smaller than 10
* samples.
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyKinematics> measurements, final double[] bias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4271 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4925 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements,
final RobustKnownGravityNormAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, listener);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, listener);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, listener);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, listener);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements list of body kinematics measurements taken at a given position with
* different unknown orientations and containing the standard deviations
* of accelerometer and gyroscope measurements.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4308 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4961 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements list of body kinematics measurements taken at a given position with
* different unknown orientations and containing the standard deviations
* of accelerometer and gyroscope measurements.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to be notified of events such as when estimation
* starts, ends or its progress significantly changes.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4387 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4549 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4548 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5194 |
final double[] qualityScores, final Double groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5038 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5195 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is smaller
* than 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Acceleration groundTruthGravityNorm,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4130 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4722 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements,
final RobustKnownPositionAccelerometerCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position, measurements, listener);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position, measurements, listener);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position, measurements, listener);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, listener);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, listener);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements list of body kinematics measurements taken at a given position with
* different unknown orientations and containing the standard deviations
* of accelerometer and gyroscope measurements.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope. If true 10 samples are
* required, otherwise 13.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than the minimum number of
* required samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4164 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4756 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position, measurements, commonAxisUsed);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position, measurements, commonAxisUsed);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position, measurements, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements list of body kinematics measurements taken at a given position with
* different unknown orientations and containing the standard deviations
* of accelerometer and gyroscope measurements.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope. If true 10 samples are
* required, otherwise 13.
* @param listener listener to be notified of events such as when estimation
* starts, ends or its progress significantly changes.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than the minimum number of
* required samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4238 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4390 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4389 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4981 |
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyKinematics> measurements, final Matrix initialBias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial bias to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or if
* quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4830 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4982 |
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(position, measurements, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if quality scores array is smaller than 13
* samples.
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyKinematics> measurements, final double[] initialBias, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3891 |
| com/irurueta/navigation/inertial/calibration/magnetometer/BaseKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 2376 |
return evaluateGeneral(i, params);
}
});
setInputData();
fitter.fit();
final var result = fitter.getA();
final var m11 = result[0];
final var m21 = result[1];
final var m31 = result[2];
final var m12 = result[3];
final var m22 = result[4];
final var m32 = result[5];
final var m13 = result[6];
final var m23 = result[7];
final var m33 = result[8];
final var mm = new Matrix(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4408 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 4704 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 4945 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5287 |
for (int i = 0, j = k; i < num; i++, j++) {
initial[j] = initG.getElementAtIndex(i);
}
return initial;
}
@Override
public double evaluate(
final int i, final double[] point, final double[] params, final double[] derivatives)
throws EvaluationException {
measAngularRateX = point[0];
measAngularRateY = point[1];
measAngularRateZ = point[2];
fmeasX = point[3];
fmeasY = point[4];
fmeasZ = point[5];
gradientEstimator.gradient(params, derivatives);
return evaluateCommonAxisWitGDependentCrossBiases(params); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java | 625 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java | 610 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustEasyGyroscopeCalibrator.java | 1360 |
| com/irurueta/navigation/inertial/calibration/gyroscope/PROSACRobustKnownBiasEasyGyroscopeCalibrator.java | 1330 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustEasyGyroscopeCalibrator.java | 697 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RANSACRobustKnownBiasEasyGyroscopeCalibrator.java | 681 |
@Override
public double getThreshold() {
return threshold;
}
@Override
public int getTotalSamples() {
return sequences.size();
}
@Override
public int getSubsetSize() {
return preliminarySubsetSize;
}
@Override
public void estimatePreliminarSolutions(
final int[] samplesIndices, final List<PreliminaryResult> solutions) {
computePreliminarySolutions(samplesIndices, solutions);
}
@Override
public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
return computeError(sequences.get(i), currentEstimation);
}
@Override
public boolean isReady() {
return MSACRobustEasyGyroscopeCalibrator.super.isReady(); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 3956 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4734 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg,
accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator(sequences, initialBias, initialMg, initialGg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4042 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustEasyGyroscopeCalibrator.java | 4832 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustEasyGyroscopeCalibrator(
sequences, commonAxisUsed, estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustEasyGyroscopeCalibrator( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2622 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 2856 |
final double biasX, final double biasY, final double biasZ,
final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, listener);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(biasX, biasY, biasZ, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param biasX known x coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasY known y coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param biasZ known z coordinate of gyroscope bias expressed in radians per
* second (rad/s).
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final List<StandardDeviationFrameBodyKinematics> measurements,
final double biasX, final double biasY, final double biasZ, final RobustEstimatorMethod method) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 4278 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 4515 |
final double[] bias, final boolean commonAxisUsed, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, bias, commonAxisUsed);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, bias, commonAxisUsed);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(measurements, bias, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, bias, commonAxisUsed);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, bias, commonAxisUsed);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements list of body kinematics measurements with standard
* deviations taken at different frames (positions, orientations
* and velocities).
* @param bias known gyroscope bias.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided quality scores length is smaller
* than 6 samples.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create(
final double[] qualityScores, final List<StandardDeviationFrameBodyKinematics> measurements,
final double[] bias, final boolean commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 4311 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 4548 |
final double[] bias, final boolean commonAxisUsed,
final RobustKnownBiasAndFrameGyroscopeCalibratorListener listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, bias, commonAxisUsed, listener);
case LMEDS -> new LMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, bias, commonAxisUsed, listener);
case MSAC -> new MSACRobustKnownBiasAndFrameGyroscopeCalibrator(
measurements, bias, commonAxisUsed, listener);
case PROSAC -> new PROSACRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, bias, commonAxisUsed, listener);
default -> new PROMedSRobustKnownBiasAndFrameGyroscopeCalibrator(
qualityScores, measurements, bias, commonAxisUsed, listener);
};
}
/**
* Creates a robust gyroscope calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param bias known gyroscope bias.
* @param method robust estimator method.
* @return a robust gyroscope calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided quality scores length is smaller
* than 6 samples.
*/
public static RobustKnownBiasAndFrameGyroscopeCalibrator create( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3466 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4252 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final Matrix initialBias, final Matrix initialMg, final Matrix initialGg, final Matrix accelerometerBias,
final Matrix accelerometerMa, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg,
initialGg, accelerometerBias, accelerometerMa);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, initialBias, initialMg, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 3552 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasEasyGyroscopeCalibrator.java | 4352 |
final List<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>> sequences,
final boolean commonAxisUsed, final boolean estimateGDependentCrossBiases, final Matrix initialBias,
final Matrix initialMg, final Matrix initialGg, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case LMEDS -> new LMedSRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case MSAC -> new MSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed,
estimateGDependentCrossBiases, initialBias, initialMg, initialGg);
case PROSAC -> new PROSACRobustKnownBiasEasyGyroscopeCalibrator(sequences, commonAxisUsed, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 4282 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 4432 |
final boolean commonAxisUsed, final double[] hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided quality
* scores length is smaller than 10 samples.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] hardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 5016 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 5179 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron);
case MSAC -> new MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, hardIron);
default -> new PROMedSRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron array does not have length 3,
* or if provided quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4271 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4942 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final RobustKnownHardIronPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, listener);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, listener);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, listener);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, listener);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param method robust estimator method
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4310 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4981 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4390 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4553 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4552 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5223 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix hardIron,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5061 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5224 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] hardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4794 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 4944 |
final boolean commonAxisUsed, final double[] initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided quality
* scores length is smaller than 10 samples.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final boolean commonAxisUsed, final double[] initialHardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 5529 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 5692 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
case LMEDS -> new LMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
case MSAC -> new MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
case PROSAC -> new PROSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
default -> new PROMedSRobustKnownMagneticFluxDensityNormMagnetometerCalibrator(
qualityScores, groundTruthMagneticFluxDensityNorm, measurements, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm expressed in Teslas (T).
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided magnetic flux norm value is negative,
* or if provided hard-iron array does not have length 3,
* or if provided quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownMagneticFluxDensityNormMagnetometerCalibrator create(
final double[] qualityScores, final Double groundTruthMagneticFluxDensityNorm,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4759 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5425 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements,
final RobustKnownPositionAndInstantMagnetometerCalibratorListener listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, listener);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, listener);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, listener);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, listener);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, listener);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param method robust estimator method
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4798 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5464 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided quality scores length
* is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4878 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5041 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5040 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5707 |
final double[] qualityScores, final NEDPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final Matrix initialHardIron,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5544 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5708 |
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final ECEFPosition position,
final List<StandardDeviationBodyMagneticFluxDensity> measurements, final double[] initialHardIron, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 103 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 155 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 989 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 1041 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 1311 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 1363 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 1630 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 1679 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 1980 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2029 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2073 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8848 |
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2117 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 8804 |
final Time timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC,
final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldPosition.getX(), oldPosition.getY(), oldPosition.getZ(),
oldC, oldVx, oldVy, oldVz, fx, fy, fz, angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVelocity previous body velocity resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate( | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2333 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2385 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECEFFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2431 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2477 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2757 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 2803 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECEFFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECEF frame containing new body
* position, velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 94 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 146 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 420 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 472 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ,
final ECIFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 592 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 644 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 761 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 810 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 859 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 908 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECIFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 960 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 1012 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ, final ECIFrame result)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 1058 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 1104 |
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECIFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 1150 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 1196 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ,
final ECIFrame result) throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param result instance where new estimated ECI frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public void navigate(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17232 |
| com/irurueta/navigation/inertial/navigators/NEDInertialNavigator.java | 17383 |
final double angularRateX, final double angularRateY, final double angularRateZ,
final double accuracyThreshold, final NEDFrame result) throws InertialNavigatorException {
try {
navigateNED(timeInterval, oldFrame.getLatitude(), oldFrame.getLongitude(), oldFrame.getHeight(),
oldFrame.getCoordinateTransformation(), oldFrame.getVn(), oldFrame.getVe(), oldFrame.getVd(),
fx, fy, fz, angularRateX, angularRateY, angularRateZ, accuracyThreshold, result);
} catch (final InvalidSourceAndDestinationFrameTypeException ignore) {
// never happens
}
}
/**
* Runs precision local-navigation-frame inertial navigation equations.
* NOTE: only the attitude update and specific force frame transformation
* phases are precise.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldFrame previous NED frame containing body position, velocity and
* coordinate transformation matrix.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param result instance where new estimated NED frame containing new body position,
* velocity and coordinate transformation matrix will be stored.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
*/
public static void navigateNED(
final double timeInterval, final NEDFrame oldFrame,
final Acceleration fx, final Acceleration fy, final Acceleration fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/AccelerationFixer.java | 1174 |
| com/irurueta/navigation/inertial/calibration/AngularRateFixer.java | 1343 |
| com/irurueta/navigation/inertial/calibration/MagneticFluxDensityFixer.java | 1257 |
final double myz, final double mzx, final double mzy, final double[] result) throws AlgebraException {
crossCouplingErrors.setElementAt(0, 0, sx);
crossCouplingErrors.setElementAt(1, 1, sy);
crossCouplingErrors.setElementAt(2, 2, sz);
crossCouplingErrors.setElementAt(0, 1, mxy);
crossCouplingErrors.setElementAt(0, 2, mxz);
crossCouplingErrors.setElementAt(1, 0, myx);
crossCouplingErrors.setElementAt(1, 2, myz);
crossCouplingErrors.setElementAt(2, 0, mzx);
crossCouplingErrors.setElementAt(2, 1, mzy); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/BaseBiasGravityNormAccelerometerCalibrator.java | 5107 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasEasyGyroscopeCalibrator.java | 3676 |
| com/irurueta/navigation/inertial/calibration/gyroscope/KnownBiasTurntableGyroscopeCalibrator.java | 5012 |
final double[] initial = new double[COMMON_Z_AXIS_UNKNOWNS];
// upper diagonal cross coupling errors M
var k = 0;
for (var j = 0; j < BodyKinematics.COMPONENTS; j++) {
for (var i = 0; i < BodyKinematics.COMPONENTS; i++) {
if (i <= j) {
initial[k] = initialM.getElementAt(i, j);
k++;
}
}
}
return initial;
}
@Override
public double evaluate(final int i, final double[] point, final double[] params,
final double[] derivatives) throws EvaluationException { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 3784 |
| com/irurueta/navigation/inertial/calibration/accelerometer/KnownFrameAccelerometerNonLinearLeastSquaresCalibrator.java | 4052 |
result[2] = bz + ftruez + sz * ftruez;
jacobian.setElementAt(0, 0, 1.0);
jacobian.setElementAt(0, 1, 0.0);
jacobian.setElementAt(0, 2, 0.0);
jacobian.setElementAt(0, 3, ftruex);
jacobian.setElementAt(0, 4, 0.0);
jacobian.setElementAt(0, 5, 0.0);
jacobian.setElementAt(0, 6, ftruey);
jacobian.setElementAt(0, 7, ftruez);
jacobian.setElementAt(0, 8, 0.0);
jacobian.setElementAt(1, 0, 0.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 818 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownPositionAccelerometerCalibrator.java | 770 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownBiasAndPositionAccelerometerCalibrator.java | 763 |
| com/irurueta/navigation/inertial/calibration/accelerometer/LMedSRobustKnownGravityNormAccelerometerCalibrator.java | 826 |
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value, the
* algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @return stop threshold to stop the algorithm prematurely when a certain
* accuracy has been reached.
*/
public double getStopThreshold() {
return stopThreshold;
}
/**
* Sets threshold to be used to keep the algorithm iterating in case that
* best estimated threshold using median of residuals is not small enough.
* Once a solution is found that generates a threshold below this value,
* the algorithm will stop.
* The stop threshold can be used to prevent the LMedS algorithm to iterate
* too many times in cases where samples have a very similar accuracy.
* For instance, in cases where proportion of outliers is very small (close
* to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
* iterate for a long time trying to find the best solution when indeed
* there is no need to do that if a reasonable threshold has already been
* reached.
* Because of this behaviour the stop threshold can be set to a value much
* lower than the one typically used in RANSAC, and yet the algorithm could
* still produce even smaller thresholds in estimated results.
*
* @param stopThreshold stop threshold to stop the algorithm prematurely
* when a certain accuracy has been reached.
* @throws IllegalArgumentException if provided value is zero or negative.
* @throws LockedException if calibrator is currently running.
*/
public void setStopThreshold(final double stopThreshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (stopThreshold <= MIN_STOP_THRESHOLD) {
throw new IllegalArgumentException();
}
this.stopThreshold = stopThreshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndFrameAccelerometerCalibrator.java | 795 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 910 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 858 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownFrameAccelerometerCalibrator.java | 280 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 917 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java | 864 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustEasyGyroscopeCalibrator.java | 692 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasAndFrameGyroscopeCalibrator.java | 806 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasEasyGyroscopeCalibrator.java | 679 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownBiasTurntableGyroscopeCalibrator.java | 1737 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustKnownFrameGyroscopeCalibrator.java | 286 |
| com/irurueta/navigation/inertial/calibration/gyroscope/MSACRobustTurntableGyroscopeCalibrator.java | 1769 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1003 |
| com/irurueta/navigation/inertial/calibration/magnetometer/MSACRobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 995 |
inliersData = null;
innerEstimator.setConfidence(confidence);
innerEstimator.setMaxIterations(maxIterations);
innerEstimator.setProgressDelta(progressDelta);
final var preliminaryResult = innerEstimator.estimate();
inliersData = innerEstimator.getInliersData();
attemptRefine(preliminaryResult);
if (listener != null) {
listener.onCalibrateEnd(this);
}
} catch (final com.irurueta.numerical.LockedException e) {
throw new LockedException(e);
} catch (final com.irurueta.numerical.NotReadyException e) {
throw new NotReadyException(e);
} catch (final RobustEstimatorException e) { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 794 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownPositionAccelerometerCalibrator.java | 746 |
super(groundTruthGravityNorm, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing bias, scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownBiasAndPositionAccelerometerCalibrator.java | 740 |
| com/irurueta/navigation/inertial/calibration/accelerometer/MSACRobustKnownGravityNormAccelerometerCalibrator.java | 801 |
super(position, measurements, commonAxisUsed, bias, initialMa, listener);
}
/**
* Returns threshold to determine whether samples are inliers or not.
*
* @return threshold to determine whether samples are inliers or not.
*/
public double getThreshold() {
return threshold;
}
/**
* Sets threshold to determine whether samples are inliers or not.
*
* @param threshold threshold to be set.
* @throws IllegalArgumentException if provided value is equal or less than
* zero.
* @throws LockedException if calibrator is currently running.
*/
public void setThreshold(final double threshold) throws LockedException {
if (running) {
throw new LockedException();
}
if (threshold <= MIN_THRESHOLD) {
throw new IllegalArgumentException();
}
this.threshold = threshold;
}
/**
* Estimates accelerometer calibration parameters containing scale factors
* and cross-coupling errors.
*
* @throws LockedException if calibrator is currently running.
* @throws NotReadyException if calibrator is not ready.
* @throws CalibrationException if estimation fails for numerical reasons.
*/
@SuppressWarnings("DuplicatedCode")
@Override
public void calibrate() throws LockedException, NotReadyException, CalibrationException {
if (running) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
} | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndFrameAccelerometerCalibrator.java | 80 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownFrameAccelerometerCalibrator.java | 83 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownBiasAndFrameGyroscopeCalibrator.java | 85 |
| com/irurueta/navigation/inertial/calibration/gyroscope/RobustKnownFrameGyroscopeCalibrator.java | 89 |
public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;
/**
* Indicates that by default preliminary solutions are refined.
*/
public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default using a non-linear calibrator
* (which uses a Levenberg-Marquardt fitter).
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Contains a list of body kinematics measurements taken at different
* frames (positions, orientations and velocities) and containing the standard
* deviations of accelerometer and gyroscope measurements.
* If a single device IMU needs to be calibrated, typically all measurements are
* taken at the same position, with zero velocity and multiple orientations.
* However, if we just want to calibrate a given IMU model (e.g. obtain
* an average and less precise calibration for the IMU of a given phone model),
* we could take measurements collected throughout the planet at multiple positions
* while the phone remains static (e.g. while charging), hence each measurement
* position will change, velocity will remain zero and orientation will be
* typically constant at horizontal orientation while the phone remains on a
* flat surface.
*/
protected List<StandardDeviationFrameBodyKinematics> measurements;
/**
* Listener to be notified of events such as when calibration starts, ends or its
* progress significantly changes.
*/
protected RobustKnownBiasAndFrameAccelerometerCalibratorListener listener; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 3943 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4745 |
final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is expressed
* in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4099 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndGravityNormAccelerometerCalibrator.java | 4591 |
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownBiasAndGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3802 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4551 |
final double[] bias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias. This must have length 3 and is
* expressed in meters per squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 3949 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownBiasAndPositionAccelerometerCalibrator.java | 4403 |
final List<StandardDeviationBodyKinematics> measurements, final boolean commonAxisUsed, final Matrix bias,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case LMEDS -> new LMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case MSAC -> new MSACRobustKnownBiasAndPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, bias);
case PROSAC -> new PROSACRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
default -> new PROMedSRobustKnownBiasAndPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, bias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param bias known accelerometer bias.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (7 or 10).
*/
public static RobustKnownBiasAndPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4470 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5271 |
final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 4627 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownGravityNormAccelerometerCalibrator.java | 5118 |
final Matrix initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownGravityNormAccelerometerCalibrator(
groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownGravityNormAccelerometerCalibrator(
qualityScores, groundTruthGravityNorm, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param groundTruthGravityNorm ground truth gravity norm expressed in meters per
* squared second (m/s^2).
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1 or
* if provided gravity norm value is negative or
* if provided quality scores length is
* smaller than 10 or 13 samples.
*/
public static RobustKnownGravityNormAccelerometerCalibrator create(
final double[] qualityScores, final Double groundTruthGravityNorm, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4314 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 5054 |
final double[] initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial accelerometer bias to be used to find a solution.
* This must have length 3 and is expressed in meters per
* squared second (m/s^2).
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias array does not have length 3
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4465 |
| com/irurueta/navigation/inertial/calibration/accelerometer/RobustKnownPositionAccelerometerCalibrator.java | 4906 |
final Matrix initialBias, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case LMEDS -> new LMedSRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case MSAC -> new MSACRobustKnownPositionAccelerometerCalibrator(
position, measurements, commonAxisUsed, initialBias);
case PROSAC -> new PROSACRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
default -> new PROMedSRobustKnownPositionAccelerometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialBias);
};
}
/**
* Creates a robust accelerometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body kinematics measures have been taken.
* @param measurements collection of body kinematics measurements with standard
* deviations taken at the same position with zero velocity
* and unknown different orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common for
* accelerometer and gyroscope.
* @param initialBias initial bias to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust accelerometer calibrator.
* @throws IllegalArgumentException if provided bias matrix is not 3x1
* or if provided quality scores length is
* smaller than the minimum number of required
* samples (10 or 13).
*/
public static RobustKnownPositionAccelerometerCalibrator create(
final double[] qualityScores, final ECEFPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/gyroscope/EasyGyroscopeCalibrator.java | 4257 |
| com/irurueta/navigation/inertial/calibration/gyroscope/TurntableGyroscopeCalibrator.java | 5621 |
final var gradientEstimator = new GradientEstimator(params -> evaluateCommonAxis(i, params));
final var initialM = Matrix.identity(BodyKinematics.COMPONENTS, BodyKinematics.COMPONENTS);
initialM.add(getInitialMg());
// Force initial M to be upper diagonal
initialM.setElementAt(1, 0, 0.0);
initialM.setElementAt(2, 0, 0.0);
initialM.setElementAt(2, 1, 0.0);
final var invInitialM = Utils.inverse(initialM);
final var initialBg = getInitialBiasAsMatrix();
final var initialB = invInitialM.multiplyAndReturnNew(initialBg);
fitter.setFunctionEvaluator(new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
@Override
public int getNumberOfDimensions() {
// Before and after normalized gravity versors
return 2 * BodyKinematics.COMPONENTS; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4531 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownFrameMagnetometerNonLinearLeastSquaresCalibrator.java | 4801 |
result[2] = bz + btruez + sz * btruez;
jacobian.setElementAt(0, 0, 1.0);
jacobian.setElementAt(0, 1, 0.0);
jacobian.setElementAt(0, 2, 0.0);
jacobian.setElementAt(0, 3, btruex);
jacobian.setElementAt(0, 4, 0.0);
jacobian.setElementAt(0, 5, 0.0);
jacobian.setElementAt(0, 6, btruey);
jacobian.setElementAt(0, 7, btruez);
jacobian.setElementAt(0, 8, 0.0);
jacobian.setElementAt(1, 0, 0.0); | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 812 |
| com/irurueta/navigation/inertial/calibration/magnetometer/KnownMagneticFluxDensityNormMagnetometerCalibrator.java | 855 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronMagneticFluxDensityNormMagnetometerCalibrator.java | 1259 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownMagneticFluxDensityNormMagnetometerCalibrator.java | 1263 |
}
/**
* Sets ground truth magnetic flux density norm to be expected at location where
* measurements have been made, expressed in Teslas (T).
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if calibrator is currently running.
*/
public void setGroundTruthMagneticFluxDensityNorm(final Double groundTruthMagneticFluxDensityNorm)
throws LockedException {
if (isRunning()) {
throw new LockedException();
}
internalSetGroundTruthMagneticFluxDensityNorm(groundTruthMagneticFluxDensityNorm);
}
/**
* Sets ground truth magnetic flux density norm to be expected at location where
* measurements have been made.
*
* @param groundTruthMagneticFluxDensityNorm ground truth magnetic flux density norm or null if undefined.
* @throws IllegalArgumentException if provided value is negative.
* @throws LockedException if calibrator is currently running.
*/
public void setGroundTruthMagneticFluxDensityNorm(final MagneticFluxDensity groundTruthMagneticFluxDensityNorm)
throws LockedException {
if (isRunning()) {
throw new LockedException();
}
if (groundTruthMagneticFluxDensityNorm != null) {
internalSetGroundTruthMagneticFluxDensityNorm(MagneticFluxDensityConverter.convert(
groundTruthMagneticFluxDensityNorm.getValue().doubleValue(),
groundTruthMagneticFluxDensityNorm.getUnit(),
MagneticFluxDensityUnit.TESLA));
} else {
internalSetGroundTruthMagneticFluxDensityNorm(null);
}
}
/**
* Indicates whether calibrator is ready to start.
*
* @return true if calibrator is ready, false otherwise.
*/
@Override
public boolean isReady() { | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownFrameMagnetometerCalibrator.java | 87 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronAndFrameMagnetometerCalibrator.java | 86 |
public static final boolean DEFAULT_USE_LINEAR_CALIBRATOR = true;
/**
* Indicates that by default preliminary solutions are refined.
*/
public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = false;
/**
* Default robust estimator method when none is provided.
*/
public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.LMEDS;
/**
* Indicates that result is refined by default using a non-linear calibrator
* (which uses a Levenberg-Marquardt fitter).
*/
public static final boolean DEFAULT_REFINE_RESULT = true;
/**
* Indicates that covariance is kept by default after refining result.
*/
public static final boolean DEFAULT_KEEP_COVARIANCE = true;
/**
* Default amount of progress variation before notifying a change in estimation progress.
* By default this is set to 5%.
*/
public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
/**
* Minimum allowed value for progress delta.
*/
public static final float MIN_PROGRESS_DELTA = 0.0f;
/**
* Maximum allowed value for progress delta.
*/
public static final float MAX_PROGRESS_DELTA = 1.0f;
/**
* Constant defining default confidence of the estimated result, which is
* 99%. This means that with a probability of 99% estimation will be
* accurate because chosen sub-samples will be inliers.
*/
public static final double DEFAULT_CONFIDENCE = 0.99;
/**
* Default maximum allowed number of iterations.
*/
public static final int DEFAULT_MAX_ITERATIONS = 5000;
/**
* Minimum allowed confidence value.
*/
public static final double MIN_CONFIDENCE = 0.0;
/**
* Maximum allowed confidence value.
*/
public static final double MAX_CONFIDENCE = 1.0;
/**
* Minimum allowed number of iterations.
*/
public static final int MIN_ITERATIONS = 1;
/**
* Contains a list of body magnetic flux density measurements taken
* at different frames (positions and orientations) and containing the
* standard deviation of magnetometer measurements.
* If a single device magnetometer needs to be calibrated, typically all
* measurements are taken at the same position, with zero velocity and
* multiple orientations.
* However, if we just want to calibrate a given magnetometer model (e.g.
* obtain an average and less precise calibration for the magnetometer of
* a given phone model), we could take measurements collected throughout
* the planet at multiple positions while the phone remains static (e.g.
* while charging), hence each measurement position will change, velocity
* will remain zero and orientation will be typically constant at
* horizontal orientation while the phone remains on a
* flat surface.
*/
protected List<StandardDeviationFrameBodyMagneticFluxDensity> measurements;
/**
* Listener to be notified of events such as when calibration starts, ends or its
* progress significantly changes.
*/
protected RobustKnownFrameMagnetometerCalibratorListener listener; | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4473 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5302 |
final double[] hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 4631 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownHardIronPositionAndInstantMagnetometerCalibrator.java | 5144 |
final Matrix hardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case LMEDS -> new LMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case MSAC -> new MSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, hardIron);
case PROSAC -> new PROSACRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
default -> new PROMedSRobustKnownHardIronPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, hardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param hardIron known hard-iron.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownHardIronPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 4961 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5786 |
final double[] initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron array does
* not have length 3 or if provided
* quality scores length is smaller
* than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5119 |
| com/irurueta/navigation/inertial/calibration/magnetometer/RobustKnownPositionAndInstantMagnetometerCalibrator.java | 5627 |
final Matrix initialHardIron, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case LMEDS -> new LMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case MSAC -> new MSACRobustKnownPositionAndInstantMagnetometerCalibrator(
position, measurements, commonAxisUsed, initialHardIron);
case PROSAC -> new PROSACRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
default -> new PROMedSRobustKnownPositionAndInstantMagnetometerCalibrator(
qualityScores, position, measurements, commonAxisUsed, initialHardIron);
};
}
/**
* Creates a robust magnetometer calibrator.
*
* @param qualityScores quality scores corresponding to each provided
* measurement. The larger the score value the better
* the quality of the sample.
* @param position position where body magnetic flux density measurements
* have been taken.
* @param measurements collection of body magnetic flux density
* measurements with standard deviation of
* magnetometer measurements taken at the same
* position with zero velocity and unknown different
* orientations.
* @param commonAxisUsed indicates whether z-axis is assumed to be common
* for the accelerometer, gyroscope and magnetometer.
* @param initialHardIron initial hard-iron to find a solution.
* @param listener listener to handle events raised by this calibrator.
* @param method robust estimator method.
* @return a robust magnetometer calibrator.
* @throws IllegalArgumentException if provided hard-iron matrix is not
* 3x1 or if provided quality scores
* length is smaller than 10 samples.
*/
public static RobustKnownPositionAndInstantMagnetometerCalibrator create(
final double[] qualityScores, final NEDPosition position, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 10209 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11143 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 10211 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11818 |
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 10212 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12185 |
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 10263 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11197 |
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11481 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12551 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11535 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12605 |
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldPosition previous cartesian position resolved along ECEF-frame axes.
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final double timeInterval, final ECEFPosition oldPosition, final CoordinateTransformation oldC, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 11819 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12998 |
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12184 |
| com/irurueta/navigation/inertial/navigators/ECEFInertialNavigator.java | 12997 |
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECEFFrame();
navigateECEF(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECEF-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECEF
* frame, resolved along ECEF-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECEF-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECEF frame,
* resolved along ECEF-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECEF frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated ECEF frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECEF-frame coordinate transformation matrix are
* invalid.
*/
public static ECEFFrame navigateECEFAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4822 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5164 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4824 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5523 |
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4825 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5626 |
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4876 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5218 |
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param kinematics body kinematics containing specific forces and angular rates applied to
* the body.
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final double timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 4878 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5574 |
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param kinematics body kinematics containing specific forces and angular rates applied to
* the body.
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final BodyKinematics kinematics) throws InertialNavigatorException, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5344 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5728 |
final double timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5398 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5782 |
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz,
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldSpeedX, oldSpeedY, oldSpeedZ, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param kinematics body kinematics containing specific forces and angular rates applied to
* the body.
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final double timeInterval, final double oldX, final double oldY, final double oldZ, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5524 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5929 |
final double angularRateX, final double angularRateY, final double angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final Acceleration fx, final Acceleration fy, final Acceleration fz,
final double angularRateX, final double angularRateY, final double angularRateZ) | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5625 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5928 |
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs.
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes and expressed in meters (m).
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldVx previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVy previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param oldVz previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes and expressed in meters per second (m/s).
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval.
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final Time timeInterval, final double oldX, final double oldY, final double oldZ,
final CoordinateTransformation oldC, final double oldVx, final double oldVy, final double oldVz,
final double fx, final double fy, final double fz, | |
| File | Line |
|---|---|
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5676 |
| com/irurueta/navigation/inertial/navigators/ECIInertialNavigator.java | 5976 |
final double fx, final double fy, final double fz,
final AngularSpeed angularRateX, final AngularSpeed angularRateY, final AngularSpeed angularRateZ)
throws InertialNavigatorException, InvalidSourceAndDestinationFrameTypeException {
final var result = new ECIFrame();
navigateECI(timeInterval, oldX, oldY, oldZ, oldC, oldVx, oldVy, oldVz, fx, fy, fz,
angularRateX, angularRateY, angularRateZ, result);
return result;
}
/**
* Runs precision ECI-frame inertial navigation equations.
*
* @param timeInterval time interval between epochs expressed in seconds (s).
* @param oldX previous cartesian x-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldY previous cartesian y-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldZ previous cartesian z-coordinate position of body frame with respect ECI
* frame, resolved along ECI-frame axes.
* @param oldC previous body-to-ECI-frame coordinate transformation.
* @param oldSpeedX previous velocity x-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedY previous velocity y-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param oldSpeedZ previous velocity z-coordinate of body frame with respect ECI frame,
* resolved along ECI-frame axes.
* @param fx specific force x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fy specific force y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param fz specific force z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in meters per squared second (m/s^2).
* @param angularRateX angular rate x-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateY angular rate y-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @param angularRateZ angular rate z-coordinate of body frame with respect ECI frame,
* resolved along body-frame axes, averaged over time interval and
* expressed in radians per second (rad/s).
* @return estimated ECI frame containing new body position, velocity and coordinate
* transformation matrix.
* @throws InertialNavigatorException if navigation fails due to numerical instabilities.
* @throws InvalidSourceAndDestinationFrameTypeException if source or destination frame types of previous
* body-to-ECI-frame coordinate transformation matrix are
* invalid.
*/
public static ECIFrame navigateECIAndReturnNew(
final double timeInterval, final Distance oldX, final Distance oldY, final Distance oldZ,
final CoordinateTransformation oldC, final Speed oldSpeedX, final Speed oldSpeedY, final Speed oldSpeedZ,
final double fx, final double fy, final double fz, | |